Jens Jauch

**Trajectory optimization based on recursive B-spline approximation for automated longitudinal control of a battery electric vehicle**

Jens Jauch

### **Trajectory optimization based on recursive B-spline approximation for automated longitudinal control of a battery electric vehicle**

#### **Karlsruher Schriftenreihe Fahrzeugsystemtechnik Band 112**

Herausgeber

#### **FAST Institut für Fahrzeugsystemtechnik** Prof. Dr.-Ing. Martin Cichon Prof. Dr. rer. nat. Frank Gauterin Prof. Dr.-Ing. Marcus Geimer Prof. Dr.-Ing. Frank Henning Prof. Dr.-Ing. Luise Kärger

Das Institut für Fahrzeugsystemtechnik besteht aus den Institutsteilen Bahnsystemtechnik, Fahrzeugtechnik, Leichtbau und Mobile Arbeitsmaschinen.

Eine Übersicht aller bisher in dieser Schriftenreihe erschienenen Bände finden Sie am Ende des Buchs.

# **Trajectory optimization based on recursive B-spline approximation for automated longitudinal control of a battery electric vehicle**

by Jens Jauch

Karlsruher Institut für Technologie Institut für Fahrzeugsystemtechnik

Trajectory optimization based on recursive B-spline approximation for automated longitudinal control of a battery electric vehicle

Zur Erlangung des akademischen Grades eines Doktors der Ingenieurwissenschaften von der KIT-Fakultät für Maschinenbau des Karlsruher Instituts für Technologie (KIT) genehmigte Dissertation

von Jens Jauch, M.Sc.

Tag der mündlichen Prüfung: 11. Juli 2023 Hauptreferent: Prof. Dr. rer. nat. Frank Gauterin Korreferent: Prof. Dr.-Ing. Eric Sax

**Impressum**

Karlsruher Institut für Technologie (KIT) KIT Scientific Publishing Straße am Forum 2 D-76131 Karlsruhe

KIT Scientific Publishing is a registered trademark of Karlsruhe Institute of Technology. Reprint using the book cover is not allowed.

www.ksp.kit.edu

*This document – excluding parts marked otherwise, the cover, pictures and graphs – is licensed under a Creative Commons Attribution 4.0 International License (CC BY 4.0): https://creativecommons.org/licenses/by/4.0/deed.en*

*The cover page is licensed under a Creative Commons Attribution-No Derivatives 4.0 International License (CC BY-ND 4.0): https://creativecommons.org/licenses/by-nd/4.0/deed.en*

Print on Demand 2024 – Gedruckt auf FSC-zertifiziertem Papier

ISSN 1869-6058 ISBN 978-3-7315-1332-2 DOI 10.5445/KSP/1000164662

# **Vorwort des Herausgebers**

Die Fahrzeugtechnik ist kontinuierlich Veränderungen unterworfen. Klimawandel, die Verknappung einiger für Fahrzeugbau und -betrieb benötigter Rohstoffe, globaler Wettbewerb, gesellschaftlicher Wandel und das rapide Wachstum großer Städte erfordern neue Mobilitätslösungen, die vielfach eine Neudefinition des Fahrzeugs erforderlich machen. Die Forderungen nach Steigerung der Energieeffizienz, Emissionsreduktion, erhöhter Fahr- und Arbeitssicherheit, Benutzerfreundlichkeit und angemessenen Kosten sowie die Möglichkeiten der Digitalisierung und Vernetzung finden ihre Antworten nicht aus der singulären Verbesserung einzelner technischer Elemente, sondern benötigen Systemverständnis und eine domänenübergreifende Optimierung der Lösungen.

Hierzu will die Karlsruher Schriftenreihe für Fahrzeugsystemtechnik einen Beitrag leisten. Für die Fahrzeuggattungen Pkw, Nfz, Mobile Arbeitsmaschinen und Bahnfahrzeuge werden Forschungsarbeiten vorgestellt, die Fahrzeugsystemtechnik auf vier Ebenen beleuchten: das Fahrzeug als komplexes, digitalisiertes mechatronisches System, die Mensch-Fahrzeug-Interaktion, das Fahrzeug in Verkehr und Infrastruktur sowie das Fahrzeug in Gesellschaft und Umwelt.

Eine automatisierte Fahrzeuglängsführung entlastet den Fahrer und erlaubt darüber hinaus eine Steigerung der Energie- und Zeiteffizienz, ohne das Situationsbewusstsein negativ zu beeinflussen. Häufig wird für die vorausschauende Optimierung der Geschwindigkeitstrajektorie die Methode der dynamischen Programmierung eingesetzt, die das globale Optimum liefert, jedoch sehr hohe Anforderungen an Rechenleistung und Speicherplatz stellt.

Herr Jauch schlägt in seiner Arbeit ein rekursives Verfahren zur Generierung optimierter Geschwindigkeitstrajektorien unter Nebenbedingungen für beliebig lange Strecken vor, dessen Rechenaufwand klein ist und lediglich linear mit der Streckenlänge steigt. Es basiert auf der Anpassung einer B-Spline-Funktion an kartenbasierte Datenpunkte und ist sowohl für lineare als auch für nichtlineare Probleme geeignet. Es liefert in der Anwendung auf batterieelektrische Pkw eine deutliche Erhöhung der Energieeffizienz bzw. der Durchschnittsgeschwindigkeit.

Karlsruhe, im Juli 2023 *Frank Gauterin*

# **Abstract**

This work describes a novel method for approximating an unbounded number of data points using a B-spline function in the linear and nonlinear weighted least squares sense. The developed method is based on iterative algorithms for state estimation and its computational effort increases linearly with the number of data points. The method allows to shift the bounded definition range of a B-spline function during run-time such that the latest data point can be considered for approximation regardless of the initially chosen definition range. Furthermore, the shift operation allows to decrease the sizes of matrices in the state estimators in order to reduce computational effort in both offline applications, in which all data points are available at once for processing, and online applications, in which additional data points are observed in each time step.

The trajectory optimization problem is formulated such that the approximation method computes a B-spline function that represents the desired velocity trajectory with respect to time using data points created from map data. The computational effort of the resulting direct trajectory optimization method increases only linearly with the unbounded temporal length of the planned trajectory. The combination with an adaptive model that describes the power train properties of a battery electric vehicle with fixed gear box transmission ratio allows to optimize velocity trajectories with respect to travel time, comfort and energy consumption.

The trajectory optimization method is extended to a driver assistance system for automated vehicle longitudinal control that is tested in simulations as well as in real test drives. Simulated drives on a chosen reference route need up to <sup>3</sup>.4 % less energy with the automated longitudinal control than with a human driver at the same average velocity. For the same energy consumption the automated longitudinal control achieves a <sup>2</sup>.6 % higher average velocity than a human driver.

# **Kurzfassung**

Diese Arbeit beschreibt ein neuartiges Verfahren zur linearen und nichtlinearen gewichteten Kleinste-Quadrate-Approximation einer unbeschränkten Anzahl von Datenpunkten mit einer B-Spline-Funktion. Das entwickelte Verfahren basiert auf iterativen Algorithmen zur Zustandsschätzung und sein Rechenaufwand nimmt linear mit der Anzahl der Datenpunkte zu. Das Verfahren ermöglicht eine Verschiebung des beschränkten Definitionsbereichs einer B-Spline-Funktion zur Laufzeit, sodass der aktuell betrachtete Datenpunkt ungeachtet des anfangs gewählten Definitionsbereichs bei der Approximation berücksichtigt werden kann. Zudem ermöglicht die Verschiebeoperation die Reduktion der Größen der Matrizen in den Zustandsschätzern zur Senkung des Rechenaufwands sowohl in Offline-Anwendungen, in denen alle Datenpunkte gleichzeitig zur Verarbeitung vorliegen, als auch in Online-Anwendungen, in denen in jedem Zeitschritt weitere Datenpunkte beobachtet werden.

Das Trajektorienoptimierungsproblem wird so formuliert, dass das Approximationsverfahren mit Datenpunkten aus Kartendaten eine B-Spline-Funktion berechnet, die die gewünschte Geschwindigkeitstrajektorie bezüglich der Zeit repräsentiert. Der Rechenaufwand des resultierenden direkten Trajektorienoptimierungsverfahrens steigt lediglich linear mit der unbeschränkten zeitlichen Trajektorienlänge an. Die Kombination mit einem adaptiven Modell des Antriebsstrangs eines batterie-elektrischen Fahrzeugs mit festem Getriebeübersetzungsverhältnis ermöglicht die Optimierung von Geschwindigkeitstrajektorien hinsichtlich Fahrzeit, Komfort und Energieverbrauch.

Das Trajektorienoptimierungsverfahren wird zu einem Fahrerassistenzsystem für die automatisierte Fahrzeuglängsführung erweitert, das simulativ und in realen Erprobungsfahrten getestet wird. Simulierte Fahrten auf der gewählten Referenzstrecke benötigten bis zu <sup>3</sup>, 4 % weniger Energie mit der automatisierten Längsführung als mit einem menschlichen Fahrer bei derselben Durchschnittsgeschwindigkeit. Für denselben Energieverbrauch erzielt die automatisierte Längsführung eine <sup>2</sup>, 6 % höhere Durchschnittsgeschwindigkeit als ein menschlicher Fahrer.

# **Acknowledgment**

This dissertation was created within the research project *e-volution* funded by the German Federal Ministry of Education and Research during my time as research associate and doctoral candidate at the Institute of Vehicle System Technology (FAST) at Karlsruhe Institute of Technology (KIT).

First, I would like to thank my adviser Professor Dr. rer. nat. Frank Gauterin for giving me the opportunity to work on this research topic and for his support as well as scientific guidance regarding research project, publications and dissertation. Furthermore, I would like to thank Professor Dr.-Ing. Eric Sax for his comprehensive feedback to this work and for being my second adviser. Moreover, I am grateful to Professor Dr.-Ing. Tobias Düser for chairing the examination committee and to my research group manager Dr.-Ing. Michael Frey for his support, suggestions and reviews.

My colleagues I would like to thank for creating an inspiring atmosphere rich of scientific exchange. In particular previous works by Felix Bleimund, Dr.-Ing. Dominik Dörr and Björn Fath facilitated the orientation of my research towards algorithms for splines and trajectories. Additionally, I gratefully acknowledge the efforts of Dr.-Ing. Johannes Masino and Dr.-Ing. Stephan Rhode for joint publications.

Within *e-volution*, I also enjoyed collaborating with Dr.-Ing. Moritz Vaillant and Volker Watteroth, associates at Dr. Ing. h.c. F. Porsche AG and Porsche Engineering Services GmbH, respectively. Their project contribution enabled the application of the developed algorithms in a real vehicle.

The students Changjoon Lee, Tim Staiger and Adam Thorgeirsson contributed to my dissertation via *e-volution*-related work and also to a mutual publication.

Special thanks go to my sister for detailed reviews of this dissertation. Lastly, I am particularly grateful to my parents, to whom I dedicate this dissertation, for their continuous support and encouragement.

Karlsruhe, July 2023 *Jens Jauch*

# **Contents**




# **Acronyms**


#### FB-KRLS Fixed-Budget KRLS




# **Symbols**

#### Optimal control .


#### Approximation







y vector of measurements.

#### Vehicle models .




xxiv



#### Trajectory planning and longitudinal control .




# **1 Introduction**

## **1.1 Background**

The topic of this work falls into the context of driver assistance systems for automated vehicle longitudinal control based on map data. Such systems can increase safety, improve comfort and reduce the energy consumption of the vehicle by computing an energy-efficient course of velocity for the road section ahead.

In the recent past, energy savings have mainly been achieved by power train improvements such as downsizing of the internal combustion engine as well as hybrid and purely electric power trains. When taking into account the increasing costs for further power train improvements, assistance systems for energy-efficient longitudinal control offer a more cost-efficient way to obtain additional energy savings. According to [117, p. 141], efficient driving has the second largest potential for energy savings (30 %) after electrification of the power train (75 %).

Driver assistance systems for automated longitudinal control (ALC) compute the desired course of velocity, also called velocity trajectory, by solving a trajectory optimization problem. Different optimization approaches are known, e.g. Dynamic Programming (DP) and direct methods (DM).

The popular DP approach finds the global optimum for a given optimization problem and its computational effort grows linearly with the temporal length of the planned trajectory. However, only coarsely discretized problems with few state dimensions can be solved in real-time on a series electronic control unit (ECU) with limited resources.

A possible way is to compute a rough long-term trajectory with DP that serves as a reference trajectory for a DM. In addition to discretizing the optimization problem, DM approximate the system state and control between the discretization steps by a function with respect to time. DM are, however, only suitable for short-term trajectories since the effort of DM increases exponentially with the temporal trajectory length.

This work addresses the need for computationally less demanding trajectory optimization algorithms for ALC of a battery electric vehicle (BEV).

## **1.2 Approach**

The research problem is approached by developing a novel method for the generic task of approximating an unbounded number of data points using a Bspline function in the linear and nonlinear weighted least squares (WLS) sense. The developed method is based on iterative algorithms for state estimation.

The approximation problem is reformulated as a trajectory optimization problem such that the approximation method computes a velocity trajectory with respect to time using data points created from map data. The novel trajectory optimization method falls into the category of DM and its effort increases only linearly with the trajectory length instead of exponentially. The combination with an adaptive model that describes the power train properties of the BEV allows to plan velocity trajectories whose resulting energy consumption varies depending on the chosen relative weighting of different target criteria.

The trajectory optimization is extented to a driver assistance system for ALC that is tested in simulation as well as in real test drives. In simulations on a chosen reference route the ALC is compared to a recorded and re-simulated real

Figure 1.1: Structure of this work illustrated by the V model that is used to describe the software development process.

drive with manual longitudinal control (MLC) regarding energy consumption and average velocity.

# **1.3 Outline**

Figure 1.1 illustrates the structure of this work, which follows the V model. The V model describes the software development process and distinguishes different process levels. On the descending branch of the V model, the development goal is analysed and iteratively broken down into subgoals. Thereby requirements are specified for subsystems that ultimately need to be implemented.

Chapter 2 of the work corresponds to the descending branch of the V model and will provide literature background as well as identify research gaps. The development goal on system level is a driver assistance system for energyefficient ALC of a BEV (Section 2.1) which is broken down into a trajectory optimization method on component level (Section 2.2) and B-spline approximation methods on algorithm level (Section 2.3). Section 2.4 gives an introductive overview of adaptive filters for Chapter 3 and Chapter 5 without identifying a research gap.

The ascending branch of the V model includes testing and iterative integration of subcomponents into larger systems until the main goal is reached. Chapter 3 to Chapter 6 address this phase by presenting contributions for closing the research gaps on each level of the V model.

Chapter 3 presents novel algorithms for iterative linear and nonlinear WLS approximation of data with a B-spline function. The algorithms are referred to as recursive B-spline approximation (RBA) and nonlinear recursive B-spline approximation (NRBA), respectively.

Chapter 4 provides neccessary foundations for Chapter 5 and Chapter 6. Without having a direct counterpart to the left branch, it states models of the vehicle that are used for determining energy-efficient trajectories and for evaluating the energy consumption of the research vehicle in simulations on a chosen reference route.

Chapter 5 applies RBA and NRBA to the trajectory optimization problem and presents extensions for taking into account trajectory constraints.

Chapter 6 describes the development process of the ALC and its system architecture including the interaction of its components. Furthermore, Chapter 6 investigates the energy-saving potential with the ALC on the reference route in comparison to a recorded and re-simulated real drive with MLC as well as effects of ALC parameters on the energy consumption that results with the ALC.

Chapter 7 concludes the journey through the V model, summarizes the contributions to research gaps and gives suggestions for future research.

# **1.4 Works, contributions and support by others**

The research described in this work took place within the research project *e-volution* [41] funded by the German Federal Ministry of Education and Research (support code 16EMO0071). The Dr. Ing. h.c. F. Porsche AG was among the research partners and supported the project work by providing a battery electric research vehicle for test drives as well as vehicle data and measurement data. Furthermore, an employee of the Porsche Engineering Services GmbH integrated the developed ALC system into the prototyping ECU of the research vehicle and conducted test drives.

The provided research vehicle was developed during the preceding research project *e-generation* [22], which was also funded by the German Federal Ministry of Education and Research (support code 16N 11865) and realized in cooperation with the above mentioned companies. The author of this work did not contribute to *e-generation* but reused the following results:

Despite differing strongly in detail, the B-spline data approximation algorithms stated in Chapter 3 adopt the iterative, filter-based characteristics of the previous method for polynomial data approximation [22, pp. 29-35], which was patented by F. Bleimund and S. Rhode [21]. A detailed differentiation between the approaches is given at the end of Chapter 3.

The adaptive traction force model (ATFM) stated in Subsection 4.7.1 stems from F. Bleimund [22, pp. 24-29] and is applied without modifications. The idea to model the power train characteristics with a kernel regression model, as done in Subsection 4.7.2 using the adaptive electrical power model (AEPM), originates from S. Rhode [146], who also provided a first script in MATLAB that performs this task. Based on this, both authors continued the investigations

independently from each other. Problem-specific adaptions for using the model for trajectory optimization as well as the determination of model hyperparameters were done by the author of this work.

Trajectory optimization within the previous ALC [22, pp. 29-35] developed by F. Bleimund differs significantly from the one described in this work, although both are examples of DM. The previous ALC defined trajectories with respect to position by a cubic polynomial. The few degrees of freedom of this function only allowed to represent trajectories with simple shape over short distances. The energy that the vehicle will need for tracking the planned trajectories was not considered in the optimization process.

Therefore, regarding trajectory optimization this work only adopts the generation of an upper speed limit (Section 5.1) from *e-generation* [22, pp. 29, 30]. The adaptions made are limited to parameterization improvements. At an early stage, with non-final trajectory representation, AEPM and nonlinear filter, tests for finding suitable target criteria weightings for the trajectory optimization including the elecrical power (Section 5.4) were done by A. Thorgeirsson [173] as a student assistant.

Moreover, reused results include agreed interfaces between the ALC system and the vehicle, the technical architecture of the ALC system as well as a Hardware-in-the-Loop (HiL) test bench, which simulates the research vehicle on the reference route and runs the developed ALC on the same ECU type as in the real research vehicle. The HiL test bench (Section 6.1) was created by B. Fath [22, pp. 60-73].

The route data module (Subsection 6.2.1) was implemented by D. Dörr [22, pp. 23, 24]. Within this work, the parameterization was enhanced and logic for processing roundabout data was added. The parameter adaption module (Subsection 6.2.2) including ATFM was developed by F. Bleimund [22, pp. 24- 29]. The author of this work added the AEPM.

To the controller module the author of this work added the functionality stated at the beginning of Subsection 6.2.4 and replaced the original proportionalintegral-derivative (PID) control by a model predictive control (MPC). Overall architecture and control loop including the pilot control (Subsubsection 6.2.4) were adopted from F. Bleimund [22, pp. 35, 36]. First implementations and simplified tests of the MPC (Subsubsection 6.2.4) were done by C. Lee [98] during his time as student assistant after completing his Master's thesis.

First evaluations of the energy-saving potential (Section 6.3) with non-final ALC setup were conducted by A. Thorgeirsson [173] while being employed as a student assistant. For more detailed differentiations regarding the individual contributions, the reader is referred to the afore-stated parts of this work.

# **1.5 Prepublications and their citations**

The research community benefits from fast publication of new findings. Therefore, parts of this dissertation have been prepublished. Namely, RBA and NRBA described in Chapter 3 are the topics of [78, 80] and the contents of Chapter 4, Chapter 5 and Chapter 6 are mentioned in a very condensed way in [78, 79].

This work will put the topics of these publications into a broader context and present them in much more detail. For more flexibility in rearranging content compared to a cumulative dissertation, only parts of previous publications have been added. To avoid interruptions of the reading flow because of frequent changes between paraphrases and direct quotes, all content adopted from prepublications, regardless of the extent of adaptions made, i.e. both paraphrases as well as direct quations, will be indicated as follows:

. . . Prepublished content . . . *Adopted from [prepublication].*

For reproducibility and convenient further use by other researchers the MATLAB source code for RBA, NRBA and trajectory planning has been provided along with the corresponding publication [75, 76, 77]. However, the source code of the trajectory planning method was published with a generic vehicle model instead of the parameters of the research vehicle and for the simplest planning case only. Therefore the results presented in Chapter 4 to Chapter 6 are not exactly reproducible by others.

## **1.6 Notation**

In this work vectors are printed in bold font and matrices are printed bold in capital letters. For example, *a* and *A* are scalars, a is a vector and A is a matrix. Unless explicitly stated, the sizes of vectors and matrices are assumed to be chosen appropriately.

A hat above a variable a such as aˆ indicates an estimate of a or the solution of an optimization problem with respect to the variable a.

This work also adopts the MATLAB-like index notation. The first index of a matrix <sup>A</sup> refers to rows, the second to columns. For example, <sup>A</sup>i,<sup>j</sup> refers to the entry in row *i* and column *j* of matrix A. The colon operator (:) refers to all rows or columns, or a range. <sup>A</sup>:,<sup>j</sup> means all rows of column *<sup>j</sup>*. Analogously, <sup>A</sup>i,: denotes all columns of row *i*. A<sup>i</sup> is an abbreviation of <sup>A</sup>i,: . The colon operator in the index of <sup>A</sup>1:4,<sup>j</sup> extracts a column vector of rows one to four in column *<sup>j</sup>* of matrix A. Additionally, a[*i*] refers to the *i*-th element of vector a.


# **2 Scientific and technical state of the art**

This chapter provides background information and identifies research gaps regarding driver assistance systems for ALC in Section 2.1, trajectory optimization in Section 2.2 and types of spline representations as well as B-spline approximation methods in Section 2.3. Furthermore, Section 2.4 gives an overview on adaptive filters for Chapter 3 and Section 4.7.

# **2.1 Assistance systems for automated longitudinal control**

The tasks that the driver performs can be divided into three levels:


Driver assistance systems can support on each of these levels.

 Regarding the vehicle guidance, assistance functions can be distinguished by their operating mode as depicted in Table 2.1 [187, pp. 36-40]:



Table 2.1: Operating modes regarding vehicle guidance [187, p. 38, adapted]

This work deals with continuously automated functions for longitudinal control. Such functions can contribute to increasing safety and comfort as well as to reducing energy consumption to different extents.

For example, the cruise control (CC) function controls the brake and engine torque so that the vehicle maintains the speed selected by the driver. Adaptive cruise control (ACC) is an extension of CC that uses a radar sensor to detect a vehicle ahead. ACC can reduce the selected vehicle velocity to maintain a chosen time gap to a vehicle ahead [52, 196]. Studies cited in [187, pp. 1140- 1145] report that with activated ACC drivers mention feeling safer and more relaxed compared to manual driving. *Adopted from [79]*. ACC-like systems increase comfort by reducing the driver workload. However, the effects on safety are not as clear as the effects on comfort and require more comprehensive investigations including psychological effects. These are adressed by works that are mentioned in the above cited literature. Briefly summarized, automation of tasks favors that the driver diverts the attention to things that are irrelevant for driving. In assistance systems that still require monitoring by the driver, this can cause a safety issue if the driver unexpectedly needs to take back control. For example, in one of the studies, drivers looked away from the traffic ahead for longer periods when driving with ACC [187, pp. 1140-1145].

Table 2.2: Time and energy savings with automated longitudinal controls that consider map data compared to benchmark control methods. Dynamic Programming (DP), direct methods (DM), conventional vehicle (CV), hybrid electric vehicle (HEV), battery electric vehicle (BEV), urban road (U), country road (C), highway (H), artificial road profile (A).


Driving efficiency oriented extensions of ACC use a corridor of allowed distances to the vehicle ahead instead of a specific distance value given by the selected time gap. Within this corridor the vehicle is controlled according to an optimization that considers the energy consumption of the vehicle, either explicitly with an energy consumption model [18, 88, 127, 195] or implicitely using a proxy for the energy consumption, such as acceleration [1, 107, 112]. The effectiveness of both approaches is compared in [81]. Section 4.7 will address consumption models in more detail.

 One step further go ACC enhancements that additionally determine the appropriate speed depending on map data so that the vehicle automatically slows down if a curve is ahead. Map information enables such systems to determine an even more energy-efficient or time-efficient driving strategy as Table 2.2 illustrates. *Adopted from [79]*.

Further developments of assistance systems increase the degree of automation and finally lead to autonomous vehicles that do not require a driver. The degree


Table 2.3: SAE J3016 levels of driving automation [172, adapted].

of automation according to SAE International is depicted in Table 2.3. Both mode A and mode C functions according to Table 2.1 are SAE level 0 functions because they are only capable of warnings and momentary assistance. Most ACC systems fall into the category SAE level 1, because they can perform longitudinal control in certain use cases but the driver has to monitor them, has to override them occasionally and needs to perform the lateral control by operating the steering wheel. Some system enhancements include lateral guidance [187, p. 1146]. Systems that offer both longitudinal and lateral control belong to at least SAE level 2.

 ALC systems that determine an energy-efficient driving strategy mainly have been developed for vehicles with internal combustion engine [139] or hybrid electric vehicles [183]. The degrees of freedom of the power train in a conventional vehicle include motor torque, clutch state and selected gear. The various operating modes of a hybrid power train translate to additional degrees of freedom. Therefore an energy-efficient driving strategy is the solution to a nonlinear and high dimensional problem. To perform the required computations on a ECU with limited computational power in real-time, the referenced approaches need to reduce the problem complexity, discretize it coarsely and partly use parallelizable, iterative and approximative approaches.

Research gap: In contrast to conventional and hybrid vehicles, a BEV usually has a 1-speed gear box and therefore a constant gear ratio. If the BEV has multiple motors, as the research vehicle does, they have at least similar characteristics so that the possible benefits from including the torque distribution in the optimization problem over a rule-based torque distribution strategy that considers efficiency and driving stability are small. With a rule-based strategy, as described in Section 4.3 for the research vehicle, the only degree of freedom that is left in the power train for optimization is the total torque. The power required by auxilaries can also be significant but will not be considered in the optimization based on the rationales mentioned in Section 4.4.

The lack of ALC systems that take advantage of this resulting simple structure by incorporating a trajectory optimization approach that is on the one hand less suitable for the complexity of the power train of a conventional or hybrid vehicle but on the other hand oriented towards low computation power demand by design, such as a local optimization based on adaptive filters, an overview of which is given in Section 2.4, poses a research gap. The next section will illustrate and compare the features of different trajectory optimization approaches in more detail. *Adopted from [78, 79]*.

# **2.2 Trajectory optimization**

Driver assistance systems that perform continuously automated functions calculate input commands to steering, brakes, engine and power train in order to achieve a desired vehicle motion, also called trajectory. Trajectory planning is also known as motion planning in robotics [187, p. 1414] and differs from a path planning in that it additionally assigns a time law to a geometric path [55]. Reviews of state of the art motion planning methods for automated driving are provided in [40, 59, 90].

With increasing extent of automation and number of degrees of freedom the decision making process of driver assistance systems becomes more complex and the trajectory is found by solving an optimization problem. Trajectory optimization denotes the process of determining a trajectory of a dynamical system including the control input to the system. Thereby the trajectory must meet the system constraints and optimize a performance measure [185, p. 3]. This section gives an overview of trajectory optimization methods.

The trajectory is often optimized with respect to a trade-off between target criteria such as comfort, safety, energy comsumption and travel time. Trajectory constraints result from the vehicle dynamics, for example power restrictions, as well as from the environment, for example lanes and other vehicles [187, p. 1414].

In static optimization problems the optimal values of a finite set of variables p need to be determined such that a cost function *J*(p) is minimized. Trajectory optimization problems fall into the category of dynamic or infinite dimensional optimization because the optimization variables are functions *x*(*t*) of an independent variable *t*, usually time. Assessing the performance of the trajectory by a scalar quantity requires a cost functional, which is a function of a function.

Figure 2.1: Methods for solving the optimal control problem

The trajectory optimization problem can be mathematically formulated as an optimal control problem (OCP):

$$\begin{aligned} \mathbf{u}^\*(t) &= \operatorname\*{arg\,min}\_{\mathbf{u}(t)} J(\mathbf{u}(t)) \\ J(\mathbf{u}(t)) &= \int\_0^{t\_f} l\left(\mathbf{x}(t), \mathbf{u}(t), t\right) \mathrm{d}t + V\left(\mathbf{x}(t\_f), t\_f\right) \\ &\dots \end{aligned} \tag{2.1}$$

subject to

$$\begin{aligned} \dot{\mathfrak{x}}(t) &= f\left(\mathfrak{x}(t), \mathfrak{u}(t), t\right), \ \mathfrak{x}(0) = \mathfrak{x}\_0, \\ \mathfrak{g}\left(\mathfrak{x}(t\_f), t\_f\right) &= \mathbf{0}, \ \ h\left(\mathfrak{x}(t), \mathfrak{u}(t), t\right) \le \mathbf{0} \ \forall t \in [0, t\_f]. \end{aligned}$$

In an OCP we seek for *<sup>t</sup>* <sup>∈</sup> [0, *<sup>t</sup>* <sup>f</sup> ] the control input trajectory <sup>u</sup>(*t*) <sup>∈</sup> <sup>R</sup> <sup>m</sup> for a system with state x ∈ R n that leads the system from its initial state x<sup>0</sup> to a terminal state xt<sup>f</sup> while minimizing the cost functional *J* and meeting the system dynamics model f , equality constraints g and inequality constraints h. u ∗ (*t*) is the optimal input trajectory and x ∗ t the resulting state trajectory. *J* includes integral costs *l* and terminal costs *V* [187, pp. 1415-1416].

 Figure 2.1 illustrates that known solution approaches to the trajectory optimization problem can be categorized into the three principles Dynamic Programming (DP), indirect methods (IM) and direct methods (DM). *Adopted from [78, 79]*. However, some approaches combine characteristics of several principles. Therefore the assignment is not necessarily unique [131, p. 27]. Apart from simple problems and special cases, solutions are derived numerically [185, p. 9]. The following subsections describe the three principles along with application examples.

#### **Dynamic Programming**

For purely continuous systems a solution to the Hamilton-Jacobi-Bellman partial differential equation provides sufficient optimality conditions for an optimal solution of the control problem. The DP algorithm samples the continuous state and control spaces in order to achieve a discrete OCP:

$$\begin{aligned} \mathbf{u}^\*(t) &= \underset{\mathbf{u}(t)}{\text{arg min}} \, J(\mathbf{u}(t)), \quad J(\mathbf{u}(t)) = \sum\_{k=0}^{k\_f - 1} l\left(\mathbf{x}(k), \mathbf{u}(k), k\right) \\\\ \text{subject to} \\ \mathbf{x}(k+1) &= f\left(\mathbf{x}(k), \mathbf{u}(k)\right), \ \mathbf{x}(0) = \mathbf{x}\_0, \ k = 0, \dots, k\_f - 1 \end{aligned} \tag{2.2}$$

The optimal control sequence u ∗ (*k*) that minimizes the sum of cost from the initial state to the final state is computed iteratively using value iteration. Equality or inequality constraints are taken into account by setting the costs *l* = ∞ for cases that violate constraints [187, pp. 1425-1430].

The algorithm takes advantage of the optimality principle of Bellman, which states that an optimal trajectory is composed of optimal subtrajectories. This principle allows to split the OCP into many smaller problems.

Since the value function depends on the system state x, the optimal control is a closed feedback control loop. The closed-loop control is valid within the bounds of the sampled state space [131, p. 5].

DP is especially beneficial for trajectory planning in driving scenarios with dynamic environment and constraints resulting from other traffic participants [87] and for planning parking maneuvers [103]. An ACC based on DP is proposed in [53]. DP based algorithms for energy-efficient ALC exist for vehicles with internal combustion engine [100, 139], hybrid electric vehicles [183], plug-in hybrid electric vehicles [197] and BEVs [188]. *Adopted from [78, 79]*.

If the time when a vehicle reaches a location is unimportant, the problem can be simplified. However, without temporal dependency the decision graph is cyclic. Therefore the value iteration needs to be replaced with another graph searching method that draws on the DP paradigm, for example the Dijkstra algorithm or the faster *A* ∗ algorithm [187, p. 1430]. In the DARPA urban challenge 2007 many successful autonomous vehicles applied an *A* <sup>∗</sup> based path planning algorithm [34].

#### **Indirect methods**

Indirect methods (IM) transform the OCP into the problem of solving a system of nonlinear equations. First, they determine the neccessary first-order optimality conditions for an OCP, which leads to a boundary value problem consisting of a set of differential equations, the so-called Hamilton equations. Then they solve the boundary value problem numerically, e.g by a Newton method [185, pp. 10, 11].

For example, variational calculus requires that the first derivative of the functional *J*(u(*t*)) diminishes for the optimal control u ∗ (*t*). The Lagrange multiplier method allows incorporating differential equality constraints resulting from the system dynamics. Without inequality constraints the Hamilton equations then read

$$\dot{\mathbf{x}} = f(\mathbf{x}, \boldsymbol{\mu}, t), \quad \dot{\boldsymbol{\lambda}} = -\frac{\partial l}{\partial \mathbf{x}} - \left[\frac{\partial f}{\partial \mathbf{x}}\right]^\top \boldsymbol{\lambda}, \quad \mathbf{0} = -\frac{\partial l}{\partial \mathbf{u}} - \left[\frac{\partial f}{\partial \mathbf{x}}\right]^\top \boldsymbol{\lambda}. \tag{2.3}$$

Herein λ denotes the Lagrange multipliers [187, pp. 1417-1418]. The approach is also known as maximum principle of Pontryagin [131, p. 32] or minimum principle of Pontryagin [185, p. 10], depending on the formulation of the optimization problem.

IM only state a necessary condition for optimality and considering state constraints is often difficult [127]. Furthermore, they require initial conditions for the Lagrange multipliers. This made them unsuitable for many automotive

applications [187, p. 1420]. Nevertheless, there are indirect trajectory optimization applications in literature, e.g. for conventional vehicles in [126, 160] and for electric vehicles in [39, 132].

#### **Direct methods**

 DM compute the solution of an OCP for a continuous system by discretizing it with respect to time. Between the discrete time steps they approximate the system state and control with a function of time. Thereby DM translate the infinite dimensional problem into a finite dimensional optimization problem. *Adopted from [79]*. System dynamics enter the optimization problem via constraints for discrete points in time. Therefore constraints are also finite.

For example, the input trajectory can be represented by a polynomial ψ and its finite dimensional parameter vector u¯:

$$
\mu(t) = \psi(t, \bar{\mu}) \tag{2.4}
$$

Then the system dynamics are

$$
\dot{\mathbf{x}}(t) = f\left(\mathbf{x}(t), \boldsymbol{\Psi}(t, \tilde{\mathbf{u}}), t\right), \\
\mathbf{x}(t\_0) = \mathbf{x}\_0. \tag{2.5}
$$

This initial value problem can be solved by a differential equation solver. The system trajectory is denoted by

$$\mathbf{x}(t) = \phi(t, \bar{\boldsymbol{\mu}}). \tag{2.6}$$

The resulting static problem that approximates the dynamic OCP reads

$$\begin{aligned} \boldsymbol{\mu}^\*(t) &= \mathop{\arg\min}\_{\boldsymbol{\tilde{u}}} J(\boldsymbol{\tilde{u}}), \\ J(\boldsymbol{\tilde{u}}) &= \int\_0^{t\_f} l\left(\boldsymbol{\phi}(t, \boldsymbol{\tilde{u}}), \boldsymbol{\psi}(t, \boldsymbol{\tilde{u}}), t\right) \mathrm{d}t + V\left(\boldsymbol{\phi}(t\_f, \boldsymbol{\tilde{u}})\right) \\ \text{subject to} \end{aligned} \tag{2.7}$$

$$\mathbf{g}\left(\phi(t\_f,\bar{\boldsymbol{u}}),t\_f\right) = \mathbf{0},\ \boldsymbol{h}\left(\phi(t\_i,\bar{\boldsymbol{u}}),\psi(t\_i,\bar{\boldsymbol{u}})\right) \le \mathbf{0},\ \boldsymbol{i} = 1,\ldots,N.$$

 The cost functional *J* is optimized directly using an optimization method that varies the finite state and control values of the functional representation. Usually the optimization problem is nonlinear and solved using sequential quadratic programming methods (e.g. [65, 92]) or interior point methods.

DM often occur in literature as MPC, which solves (2.7) on a receding horizon [187, pp. 1420-1425,1431-1432]. *Adopted from [78]*. For example, [56] applies MPC for generating locally optimal trajectories, [28] proposes a MPC based lateral control that tracks the lane centerline while avoiding collisions, [47] calculates the control inputs with MPC so that the vehicle tracks a planned path and [127] uses MPC for an ACC system that additionally aims to reduce the energy consumption.

#### **Comparison**

A comparison of the stated solution principles is presented in [131, p. 8] and further summarized in Table 2.4. The generalized statements apply to many methods found in literature but individual methods might differ.

DP computes a globally optimal control because of the sufficient optimality conditions of the Hamilton-Jacobi-Bellman equations. The computational effort grows linearly with increasing time horizon and number of discrete states. DP offers a closed-loop feedback control that is beneficial for the system stability. DP algorithms converge globally in the discrete state space. However, the required discretization leads to an exponential increase of computational effort

Table 2.4: Comparison of methods for solving an optimal control problem (OCP). The table shows average values for common algorithms applied to the same OCP. Some methods may not fulfill all of the generalized statements. Summarized version derived from [131, p. 8]. <sup>∗</sup> : According to [131, p. 8], an exception that achieves polynomial increase using a convexification approach is stated in [151].


with the number of continuous states. Therefore, DP usually can only be applied to systems with a state and control dimension up to four and requires coarse discretization. Therefore the solution accuracy is comparatively low. If the discretization is refined for more accurate solutions, the computational effort increases polynomially. DP requires a high system knowledge and can solve less classes of OCP than other principles.

IM are very accurate and the computational effort only grows linearly when the accuracy is increased. With respect to the time horizon and number of states, the complexity grows polynomially. IM can solve more OCP classes than DP but less than DM. IM only compute locally optimal open-loop controls which means that the control loop is not closed using system feedback information. IM converge within a smaller domain than DM and in general, applying IM requires more knowledge than other methods.


Table 2.5: Suitability of Dynamic Programming (DP), direct methods (DM), indirect methods (IM) and combinations for selected problem features. Particularly suitable (++), suitable (+), less suitable (−). Derived from [187, p. 1430].

 DM converge within a larger domain than IM. They can deal with numerous states, require less control knowledge and can solve more OCP classes than other methods. Like IM, DM provide only locally optimal open-loop control. The acceptable solution accuracy mainly results from discretization errors. The computational effort grows polynomially with the solution accuracy and the number of states. With respect to the time horizon, effort usually grows exponentially because in each time step all controls have to be analyzed. Therefore the optimization horizon is mostly restricted to few seconds [131, pp. 2-12, 27- 37], [187, pp. 1415-1431].

Due to their complementary properties, different approaches are combined for solving difficult, farsighted trajectory optimization problems. Then DP provides a rough long-term plan or reference trajectory for a DM that computes feasible trajectories within a short time horizon. *Adopted from [78, 79]*. An example is the energy-optimal adaptive cruise control in [186]. Closed-form solutions of calculus of variations can be applied to speed up DP. Table 2.5 shows the benefits of combining different methods [187, pp. 1430-1431].

Research gap: Both DP and DM are popular for automotive applications on their own and combining their orthogonal features offers great potential. However, the exponential growth of computational effort with increasing time horizon limits the application of DM to short time horizons. Hence, the research gap regarding trajectory optimization consists of available DM with lower complexity.

### **2.3 B-spline data approximation**

Data approximation can be viewed as a special case of trajectory optimization using the DM approach and instead of a polynomial, a B-spline function can represent the input trajectory from (2.4) or the system trajectory from (2.6). Following an overview of spline representations, this section states methods for determining a data approximating B-spline function and identifies the corresponding research gap.

#### **Spline representations and their features**

A function that is used for approximating data points needs to possess a sufficient number of parameters which translates to the number of degrees of freedom. A complicated relationship in the data usually requires more function parameters in order to achieve an acceptable representation.

With polynomials the degree is coupled to the number of parameters and as the degree is increased, the polynomials become computationally expensive to evaluate [5]. Assume a polynomial *p*(*u*) given by

$$p(\boldsymbol{\mu}) = \sum\_{i=0}^{d} \boldsymbol{\mu}^{i} \boldsymbol{c}\_{i}. \tag{2.8}$$

With a high degree *d* the monomial basis functions *u* d , *u* d−1 , . . . tend to take large values and therefore need to be multiplied with small coefficient values *<sup>c</sup>*d, *<sup>c</sup>*d−1, . . . . This can lead to ill-conditioned matrices, from which numerical instability issues can arise [35, pp. 272-274]. Increasing the degree of a polynomial approximation function also tends to result in an undesired oscillating behavior instead of better accuracy [35, pp. 292-294]. This effect is called Runge's phenomenon [150].

Figure 2.2: Curve segment (left) and surface patch (right) [5, pp. 508-509,adapted].

The explicit form of a curve y = *f* (*x*) in two dimensions, *x*, y, states the value of one dependent variable, y, in terms of the independent variable, *x*. Most curves can also be represented in the implicit form *<sup>f</sup>* (*x*, <sup>y</sup>) <sup>=</sup> <sup>0</sup>.

However, curves in three dimensions are not as easily defined in implicit form. Instead, they are usually represented in parametric form in terms of an independent variable *u*, called parameter. An advantage of the parametric form is that each dimension is defined by an explicit function that is not coupled to the others:

$$\mathbf{p}(\boldsymbol{\mu}) = \begin{bmatrix} \mathbf{x}(\boldsymbol{\mu}), \ \mathbf{y}(\boldsymbol{\mu}), \ \mathbf{z}(\boldsymbol{\mu}) \end{bmatrix}^{\mathsf{T}} \tag{2.9}$$

For example, a cubic polynomial parametric curve p(*u*) reads

$$\begin{aligned} \mathbf{p}(\boldsymbol{\mu}) &= \sum\_{k=0}^{3} \boldsymbol{\mu}^{k} \mathbf{c}\_{k} = \boldsymbol{\mu}^{\top} \mathbf{c}, \ \mathbf{u} = \begin{bmatrix} 1, \ \boldsymbol{u}, \ \boldsymbol{u}^{2}, \ \boldsymbol{u}^{3} \end{bmatrix}^{\top}, \\ \mathbf{c}\_{k} &= \begin{bmatrix} c\_{\boldsymbol{x}k}, \ c\_{\boldsymbol{y}k}, \ c\_{\boldsymbol{z}k} \end{bmatrix}^{\top}, \ \mathbf{c} = \begin{bmatrix} \mathbf{c}\_{0}, \ \mathbf{c}\_{1}, \ \mathbf{c}\_{2}, \ \mathbf{c}\_{3} \end{bmatrix}^{\top}. \end{aligned} \tag{2.10}$$

c is viewed as a 4 × 1 matrix, whose elements c<sup>k</sup> are 3 × 1 matrices. Each coefficient vector c<sup>k</sup> has independent components for each dimension, hence there are three independent functions, each of the form (2.8) with *d* = 3. Without loss of generality, 0 ≤ *u* ≤ 1 can be assumed. Figure 2.2 illustrates such a curve segment.

Defining each explicit function in terms of two parameters *u* and v, leads to a parametric surface <sup>p</sup>(*u*, <sup>v</sup>), e.g. for three dimensions:

$$\mathbf{p}(\boldsymbol{\mu}, \boldsymbol{\nu}) = \begin{bmatrix} \boldsymbol{x}(\boldsymbol{\mu}, \boldsymbol{\nu}), \ \mathbf{y}(\boldsymbol{\mu}, \boldsymbol{\nu}), \ \boldsymbol{z}(\boldsymbol{\mu}, \boldsymbol{\nu}) \end{bmatrix}^{\top} \tag{2.11}$$

A surface patch, as also depicted by Figure 2.2, can be defined by varying *u* and <sup>v</sup> over the rectangle <sup>0</sup> <sup>≤</sup> *<sup>u</sup>*, <sup>v</sup> <sup>≤</sup> <sup>1</sup>. A bicubic surface patch is given by

$$\begin{aligned} \mathbf{p}(\boldsymbol{\mu}, \boldsymbol{\nu}) &= \sum\_{i=0}^{3} \sum\_{j=0}^{3} \boldsymbol{\mu}^{i} \boldsymbol{\nu}^{j} \mathbf{c}\_{ij} = \mathbf{u}^{\top} \mathbf{C} \boldsymbol{\nu}, \\ \mathbf{v} &= \begin{bmatrix} 1, \ \boldsymbol{\nu}, \ \boldsymbol{\nu}^{2}, \ \boldsymbol{\nu}^{3} \end{bmatrix}^{\top}, \ \mathbf{c}\_{ij} = \begin{bmatrix} c\_{\boldsymbol{\nu}ij}, \ c\_{\boldsymbol{\nu}ij}, \ c\_{\boldsymbol{\nu}ij} \end{bmatrix}^{\top} \end{aligned} \tag{2.12}$$

C = f ci jg is a 4x4 matrix with elements ci j. A surface patch can be seen as the limit of a collection of curves that result from keeping one parameter constant and varying the other [5, pp. 503-515].

Spline functions are piecewise defined, which decouples the number of parameters from the degree of the function. Spline functions allow to increase the number of function parameters while still using polynomials of low degree in order to avoid numerical stability issues and Runge's phenomenon. At the join points, neighboring pieces of a spline function are continuously differentiable to a certain extent. A function that is continuously differentiable up to its *r* th derivative is called a *C* r continuous function [35, p. 26].

The remainder of this subsection reviews the most common cubic spline types. Figure 2.3 illustrates them and Table 2.6 compares their features.

A straight-forward approach to construct a cubic spline function is to connect cubic polynomials by specifying continuity requirements at their join points as explained in [35, pp. 294-299]. In literature this approach is also referred to as natural polynomial spline [9].

For example, connecting two cubic polynomials p(*u*) and q(*u*) while demanding continuity up to the second derivative leads to eight degrees of freedom,

Figure 2.3: Cubic curve segments p(u), and q(u) of various representation types depending on parameter <sup>u</sup> <sup>∈</sup> [0, 1] with their control points <sup>p</sup><sup>i</sup> , qi , <sup>i</sup> <sup>=</sup> <sup>0</sup>, . . ., <sup>3</sup> indicated by dots. Dotted lines indicate borders of convex hulls defined by the control points. [5, pp. 510- 534, adapted].

which need to fulfill three equality constraints at the join point. Figure 2.3a illustrates this case. In order to determine possible spline functions, an undetermined linear system of equations needs to be solved. Each additional polynomial segment leads to four additional degrees of freedom and three additional continuity constraints. Furthermore, the parameters of all segments of the same dimension influence each other via the continuity contraints and therefore changing the spline function at any point can affect the shape of the


Table 2.6: Cubic representation types for functions, curves and surfaces.

function at any other point. This feature is called *global control* and mostly undesired because it requires that all segments are designed within a single global calculation.

In contrast, the spline types described below offer *local control* of shape, meaning that changing a parameter only influences the function locally. Therefore each segment can be designed individually. Furthermore, they achieve continuity without solving a system of equations during application.

Geometrically interpretable parameters called *control points* are used to design the shape of these functions. Assume four control points

$$\mathbf{p}\_k = \begin{bmatrix} \mathbf{x}\_k, \ \mathbf{y}\_k, \ z\_k \end{bmatrix}^\top, \ k = 0, 1, 2, 3,\tag{2.13}$$

which are equally spaced at *<sup>u</sup>* <sup>=</sup> <sup>0</sup>, 1/3, 2/3, <sup>1</sup>. <sup>c</sup> is determined such that <sup>p</sup>(*u*) from (2.10) interpolates the control points p<sup>k</sup> . The conditions

$$\mathbf{p}\_0 = \mathbf{p}(0), \ \mathbf{p}\_1 = \mathbf{p}(1|\mathfrak{z}), \ \mathbf{p}\_2 = \mathbf{p}(2|\mathfrak{z}), \ \mathbf{p}\_3 = \mathbf{p}(1) \tag{2.14}$$

read in matrix form

$$\boldsymbol{p} = \boldsymbol{A}\boldsymbol{c}, \ \boldsymbol{p} = \begin{bmatrix} \boldsymbol{p}\_{0} \\ \boldsymbol{p}\_{1} \\ \boldsymbol{p}\_{2} \\ \boldsymbol{p}\_{3} \\ \boldsymbol{p}\_{3} \end{bmatrix}, \ \boldsymbol{A} = \begin{bmatrix} \boldsymbol{\mu}(\boldsymbol{0})^{\top} \\ \boldsymbol{\mu}(\boldsymbol{1}/\boldsymbol{\upbeta})^{\top} \\ \boldsymbol{\mu}(\boldsymbol{2}/\boldsymbol{\upbeta})^{\top} \\ \boldsymbol{\mu}(\boldsymbol{1})^{\top} \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 1 & \mathbf{1}/\boldsymbol{\upbeta} & \begin{pmatrix} \mathbf{1}/\boldsymbol{\upbeta} \end{pmatrix}^{2} & \begin{pmatrix} \mathbf{1}/\boldsymbol{\upbeta} \end{pmatrix}^{3} \\ 1 & \mathbf{2}/\boldsymbol{\upbeta} & \begin{pmatrix} \mathbf{2}/\boldsymbol{\upbeta} \end{pmatrix}^{2} & \begin{pmatrix} \mathbf{2}/\boldsymbol{\upbeta} \end{pmatrix}^{3} \\ 1 & 1 & 1 & 1 \end{pmatrix} \end{bmatrix}. \tag{2.15}$$

p and c are 4 × 1 matrices, whose elements are 3 × 1 matrices. The control points given by c = A <sup>−</sup><sup>1</sup> p = M p with geometry matrix M = A −1 achieve continuity at join points, but not for the derivatives. Figure 2.3b shows two curve segments p(*u*), q(*u*) with their control points [5, pp. 509-517].

For *C* 2 continuity, quintic polynomials would have to be used. Increasing the degree of a polynomial segment from three on complicates the calculation process with each increment significantly [11]. In particular, for polynomials of degree higher than four there is no general closed-form solution [23]. This makes nonnegativity checks by determining the roots costly.

By substituting M into (2.10), p(*u*) can be expressed in terms of the blending polynomials b(*u*) with

$$b(\boldsymbol{u}) = \boldsymbol{\mathfrak{u}}^{\top} \boldsymbol{M} = [b\_0(\boldsymbol{u}), \ b\_1(\boldsymbol{u}), \ b\_2(\boldsymbol{u}), \ b\_3(\boldsymbol{u})] \tag{2.16}$$

and the control point vector p:

$$\mathbf{p}(\boldsymbol{\mu}) = \boldsymbol{\mathfrak{u}}^{\top}\mathbf{c} = \boldsymbol{\mathfrak{u}}^{\top}(\mathbf{M}\,\mathbf{p}) = (\boldsymbol{\mathfrak{u}}^{\top}\mathbf{M})\mathbf{p} = \boldsymbol{\mathfrak{b}}(\boldsymbol{\mathfrak{u}})\mathbf{p} \tag{2.17}$$

Interpolating curves can be extended to interpolating surfaces. Assume a 4 × 4 control point matrix P = f pi jg comprising 16 independent three-dimensional control points <sup>p</sup>i j, *<sup>i</sup>* <sup>=</sup> <sup>0</sup>, . . ., <sup>3</sup>, *<sup>j</sup>* <sup>=</sup> <sup>0</sup>, . . ., <sup>3</sup>, which are again equally spaced at *<sup>u</sup>*, <sup>v</sup> <sup>=</sup> <sup>0</sup>, 1/3, 2/3, <sup>1</sup>. The curve for <sup>v</sup> <sup>=</sup> <sup>0</sup> must interpolate <sup>p</sup>00, <sup>p</sup>10, <sup>p</sup>20, <sup>p</sup>30:

$$\mathbf{p}(\boldsymbol{\mu},0) = \boldsymbol{\mathfrak{u}}^{\top}\boldsymbol{\mathcal{M}} \begin{bmatrix} \mathbf{p}\_{00}^{\top}, \ \mathbf{p}\_{10}^{\top}, \ \mathbf{p}\_{20}^{\top}, \ \mathbf{p}\_{30}^{\top} \end{bmatrix}^{\top} = \boldsymbol{\mathfrak{u}}^{\top}\boldsymbol{\mathbf{C}} \begin{bmatrix} 1, \ 0, \ 0, \ 0 \end{bmatrix}^{\top} \tag{2.18}$$

<sup>v</sup> <sup>=</sup> <sup>1</sup>/3, 2/3, <sup>1</sup> give the other three interpolating curves. Putting all of them together leads to u <sup>&</sup>gt;MP = u <sup>&</sup>gt;C A<sup>&</sup>gt; with A <sup>−</sup><sup>1</sup> = M. Solving for coefficient matrix C = MPM<sup>&</sup>gt; and substituting with (2.16) results in

$$\mathbf{p}(\boldsymbol{u}, \boldsymbol{\nu}) = \mathbf{u}^{\top} \mathbf{M} \mathbf{P} \mathbf{M}^{\top} \boldsymbol{\nu} = \mathbf{b}(\boldsymbol{u}) \mathbf{P} \mathbf{b}(\boldsymbol{\nu})^{\top} = \sum\_{i=0}^{3} \sum\_{j=0}^{3} b\_{i}(\boldsymbol{u}) b\_{j}(\boldsymbol{\nu}) \mathbf{p}\_{ij}. \tag{2.19}$$

Each term *b*i(*u*)*b*j(v) describes a blending patch. The surface is formed from 16 blending patches, each weighted by a control point. Surfaces that are created in such a way are called *tensor product surfaces* and are an example of separable surfaces. They allow to work with functions in *u* and v independently [5, pp. 510-517].

The spline types described in the following can also be represented using (2.17) and (2.19). They differ only in the blending functions b(*u*) from (2.16), because their designs fulfill conditions other than (2.14) and (2.18).

Hermite curves depicted in Figure 2.3c offer *C* 1 continuity between different segments. However, instead of p1, p2, the user needs to specify the values at the join points up to their first derivatives p 0 (0) and p 0 (1), so that consecutive tangents are collinear. This is often undesired because without an analytic formulation of the data, no derivative information is available.

Bézier curves are approximations to Hermite curves and do not need derivative information. They result when the derivatives of two segments at their join point are not required to be exactly equal. Therefore cubic Bézier curves only have *C* 0 continuity. Each segment is within the convex hull given by its control points, see Figure 2.3d. For spline types with this convex hull feature, only the discrete control points but not the continuous curve itself need to be evaluated to ensure that the curve fulfills certain constraints, e.g. nonnegativity.

For higher continuity, an alternative to increasing the polynomial degree is to not require the curve to interpolate any control point. This leads to B-spline curves (Figure 2.3e). Each of their segments only spans the distance between its two middle control points. In general they only come close to the control points but compensate with *C* 2 continuity at the join points.

Omitting the requirement that each spline segment must lie within the convex hull of its control point allows to form other types of splines. One of the most popular is the *C* 1 continuous Catmull-Rom spline (Figure 2.3f) that interpolates its control points. Compared to a Hermite curve it does also not require derivative information [5, pp. 509-535].

The remainder of this work will use cubic B-spline functions. Compared to natural splines, they offer numerically stable computations, local control and do not require solving a system of equations during their application. Compared to the other cubic spline functions, only B-splines are both *C* 2 continuous and fulfill the convex hull property. *C* 2 continuity is an important requirement for technical applications such as the definition of a jerk-free trajectory [55].

The convex hull property in combination with the geometrically interpretable control points offers a convenient and computationally efficient way to enforce constraints on the function because no evaluation of the function is needed. Enforcing constraints on an approximation function can be beneficial in case of knowledge about the data, e.g. that it is nonnegative.

 Due to their favorable features, B-spline functions, curves and surfaces are widely used for data approximation [4, 84, 202] and for defining paths and trajectories of vehicles [17, 42, 45], robots [26, 45, 108, 162] and industrial machines [69, 200]. Moreover, the B-spline representation is common in computer graphics [93, 193] as well as in signal processing for filter design [128, 129, 148] and signal representation [120, 142, 143, 176, 177]. *Adopted from [78, 80]*.

### **Methods for B-spline data approximation**

 According to (2.17), the value of a B-spline function equals the sum of basis functions (B-splines) or blending functions, weighted with their corresponding control points. Each B-spline in (2.16) is only nonzero within a bounded interval so that effects of control point changes are only local. Hence, the definition range of a B-spline function is also bounded. More extensively these features will be described in Section 3.1 and by Figure 3.2. *Adopted from [80]*.

 A common application of B-spline functions, curves and surfaces is fitting of data points. Fitting can either be interpolation or approximation. An interpolating B-spline function *<sup>f</sup>* must pass through all of the data points (*s*p, <sup>y</sup>p), i.e. *f* (*s*p) = y<sup>p</sup> ∀*p*. In contrast, an approximating B-spline function only minimizes the residuals *f* (*s*p) − y<sup>p</sup> between the function and the data but does not pass through the data points in general.

In *offline applications*, in which all data points are available at once, fitting B-spline functions are often determined by least squares (LS) methods [31, 113, 159]. With the standard formula in batch form, all data points have to be collected first and then processed simultaneously. Therefore the number of data points *P* must be bounded. The computation usually involves a Cholesky or QR factorization and requires O(*P*) operations if one takes advantage of the banded matrix structure [20, pp. 327-331]. Such algorithms are stated in [20, pp. 117-121] and [58, pp. 152-160]. With the least squares (LS) algorithm each data point influences the result to the same extent. The weighted least squares (WLS) estimator, stated in (3.14), allows to weight measurements relative to each other [113, pp. 119-123].

In *online applications* data points are observed one after another and an evergrowing amount of data is common. Two groups of LS algorithms for online applications can be distinguished: First, growing memory LS algorithms apply an exponential weighting that forgets old data. Second, sliding window LS algorithms discard old data completely and need only finite storage [201]. Sliding window LS and sliding window WLS algorithms are proposed in [83, 201] and [33, 184], respectively. Re-computing the fitting function from scratch with each new data point is costly. Rank update and rank downdate methods allow to reuse an already known factorization for an efficient update of a solution after observations have been added or deleted [66, 124, 125].

In online applications data points can be outside the definition range of the B-spline function if the magnitude of the data points is not exactly known or changes over time, e.g. because one dimension of the data point refers to its observation time, that keeps increasing. Data points outside the definition range cannot be taken into account for approximation. Thereby the problem arises that the approximation might not reflect the data anymore. When using WLS the bounded definition range of B-spline functions does not present a problem because the number and position of B-splines can be changed if the fitting function is re-computed from scratch. Moreover, rank modification methods also support adding or deleting matrix columns [66]. This allows to extend, shrink or shift the definition range of the B-spline function. *Adopted from [80]*.

 In nonlinear weighted least squares (NWLS) problems, the solution depends on the function parameters in a nonlinear fashion. Based on the results of numerical experiments, [73] reports that a B-spline function is useful for solving NWLS problems as well because of its piecewise polynomial character and smoothness. For NWLS problems, several batch methods that work iteratively can be found in literature, e.g. the Newton method, Gauss-Newton method, Levenberg-Marquardt method, dog leg method of Powell, hybrid method of Madsen, Levenberg-Marquardt-Fletcher method. None of the algorithms is an exact method that computes an optimal solution [73]. A method for separable NWLS problems in which some parameters affect the solution linearly is derived in [149].

The Levenberg-Marquardt (LM) algorithm, described in more detail in Subsection 3.4.1, solves in each iteration a linearized NWLS problem [35, pp. 222- 224]. A sliding window implementation of the LM algorithm is stated in [38]. *Adopted from [78]*.

 Recursive methods compute an approximating B-spline function recursively meaning that the approximation is updated with each new data point. This approach is preferred in online applications, in which data points are observed one after another. Recursive algorithms such as recursive least squares (RLS) [164, pp. 84-88] usually require less computational power than batch algorithms because they use smaller matrices and vectors whose sizes do not depend on the number of data points. The recursive computation is also referred to as progressive, iterative or sequential [110, 192]. This work will use all mentioned terms synonymously. In computer science, however, an iteration differs from a recursion. In [106], fitting B-spline curves and surfaces are iteratively constructed based on the idea of profit and loss modification without solving a linear system. The authors of [36] build on the progressive and iterative approximation technique for B-spline curve and surface fitting and prove that their algorithm achieves a least squares fit to the data points. A recursive algorithm for optimal smoothing B-spline surfaces inspired by the RLS method is presented in [50]. Algorithms that involve a Kalman filter (KF) are stated in [67, 104]. *Adopted from [80]*.

Recursive algorithms for NWLS problems can be based on nonlinear filters. Overviews of commonly used filters and their features are provided by [57] and [187, p. 615]. In [2] a solution via a modified extended Kalman filter (EKF) is investigated and algorithms for offline and online applications are stated. The two-step estimator proposed in [68] splits the problem into a linear subproblem which is solved with a KF and a nonlinear subproblem to which the Gauss-Newton algorithm is applied. Section 2.4 will give an introduction into the aforementioned adaptive filters and categorize them.

Research gap: Publications of other researchers regarding the recursive data approximation with a B-spline function assume a constant definition range. For example, the approaches based on the KF in [67, 104] require that the KF state vector contains all control points that are estimated during the whole approximation procedure. Therefore the number of control points has to be bounded and specified in advance. As a result, these algorithms can only approximate data points that are within the bounded B-spline function definition range determined at the beginning. Other researchers have not addressed the possible issue of data points outside the inital B-spline function definition range. *Adopted from [80]*.

Therefore the research gap regarding the recursive data approximation with a B-spline function consists of the lack of available online methods for data approximation with B-spline functions in the linear WLS and NWLS sense that can handle data that leaves the initially chosen bounded B-spline function definition range. For example, leaving the initial definition range is needed if an additional data point of an unbounded data set is observed in each time step and the values in at least one dimension of the data points keep increasing because they refer to the observation time. More detailed application examples are stated on page 78 and illustrated by Figure 3.9.

## **2.4 Adaptive filters**

 As a foundation for Chapter 3 and Chapter 5 this section gives an overview of several kinds of adaptive filters with focus on the Bayesian approach to state estimation, which calculates the probability density function (PDF) of the unknown state of a dynamic system. The required information for calculating the PDF stems partly from a system model and partly from previous measurements. The state estimation is performed by a recursive filter that alternates between a time update that predicts the state via the system model and a measurement update that corrects the estimate with the current measurement. *Adopted from [78]*.

Assume a system whose input *u* and output y are measurable and known for a sequence of time steps *<sup>p</sup>* <sup>=</sup> <sup>1</sup>, . . ., *<sup>P</sup>*.

Adaptive filters include a system model ˆ*f* with free model parameter *x*. Sequentially they adjust *x* based on the sequence of *u* and y such that the parameter estimate *<sup>x</sup>*<sup>ˆ</sup> minimizes the residuals between the system model output <sup>y</sup><sup>ˆ</sup> <sup>=</sup> <sup>ˆ</sup>*<sup>f</sup>* (*x*ˆ, *<sup>u</sup>*) and y [111, p. 1].

*Filtering* means that *x*ˆp, referring to time step *p*, is calculated based on data of the current and all previous time steps, hence *p* = *P*. This is also referred to as measurement update. The estimation problem is called *prediction* in case of *<sup>p</sup>* > *<sup>P</sup>* and *smoothing* if *<sup>p</sup>* < *<sup>P</sup>* [72, p. 7].

Adaptive filters can be subdivided according to their system assumptions. In systems, in which *x* is assumed to change over time, it is also called *system state*, whereas *x* is mostly referred to as *parameter* in systems, in which it is assumed to be constant.

### **Linear adaptive filters**

 RLS and KF compute an optimal state estimate for systems with linear system and measurement equations as well as Gaussian system and measurement noise. They differ in that RLS assumes a constant state, whereas the KF is designed for tracking a time-variant state [164, p. 129], [57, pp. 3-5]. Use cases include parameter estimation [83] and path planning [189], respectively. The KF will be explained in Subsection 3.3.2 and applied within RBA in Subsection 3.3.3.

### **Nonlinear adaptive filters**

In many scenarios the linear Gaussian assumptions do not apply and suboptimal approximate nonlinear Bayesian filters such as the extended Kalman filter (EKF), unscented Kalman filter (UKF) or particle filter (PF) are required [8].

The EKF applies a local first order Taylor approximation to the nonlinear system and measurement functions via Jacobians, in order to keep the linear state and measurement equations. System and measurement noise are both approximated with zero-mean Gaussian PDFs [57, p. 52]. Although the EKF is not suitable for systems with strong nonlinearity or non-Gaussian noise, it is still often successfully used for nonlinear state estimation [27]. For example, NWLS approximation via a modified EKF is presented in [2].

An alternative to the approximation of nonlinear state and measurement functions is the approximation of the PDFs. This can be done by propagating few

state samples called sigma points through the nonlinear functions. A filter that follows this approach is referred to as sigma point Kalman filter. A well known representative is the UKF. It uses 2· *J* +1 deterministically chosen sigma points, whereby *J* denotes the system state dimensions. The PDFs are approximated as Gaussians whose means and variances are determined from the propagated sigma points [57, pp. 3-5, 62-70].

Compared to the EKF, the UKF offers at least second order accuracy [37] and is a derivative free filter [57, pp. 62-63], meaning that it does not require the evaluation of Jacobians, which is often computationally expensive in the EKF [27]. Several publications report nonlinear problems in which the UKF performs better than the EKF, e.g. for trajectory estimation [37, 63]. However, if the PDF cannot be well approximated by a Gaussian, because the PDF is multimodal or has a strong skew, the UKF will also not perform well. Under such conditions, sequential Monte Carlo methods like the PF outperform Gaussian filters like EKF and UKF [8].

The PF approximates the PDF by a large set of randomly chosen state samples called particles. The state estimate is a weighted average of the particles. With increasing number of particles the PDF approximation by the particles becomes equivalent to the functional PDF representation and the estimate converges against the optimal estimate [8]. For nonlinear and non-Gaussian systems the PF allows to determine various statistical moments, whereas EKF and UKF are limited to the approximation of the first two moments [27]. However, the number of particles that is needed for sufficient approximation of the PDF increases exponentially with the state dimension [122]. The PF has been successfully applied to optimization [99] and prediction [190] of trajectories as well.

Many use cases involve a mixed linear/nonlinear system. Typically there are few nonlinear state dimensions and comparatively many linear Gaussian state dimensions. The marginalized particle filter (MPF) is beneficial for such problems as it combines KF and PF. The PF is only applied to the nonlinear states because the linear part of the state vector is marginalized out and optimally filtered with the KF.

This state decomposition is called Rao-Blackwellization [72, pp. 49-50] and can be described as an optimal Gaussian mixture approximation. Therefore the MPF is also called Rao-Blackwellized particle filter [70]. Marginalizing out linear states from the PF strongly reduces the computational effort because less particles suffice. This often enables real-time applications. Simultaneously the estimation accuracy usually increases [27, 205]. When the state dimension is large, the MPF tends to outperform the PF [123].

In the recent past, several publications have proposed approaches for localization [135, 191] and trajectory tracking [109, 205] that are based on the MPF because of its advantages for mixed linear/nonlinear systems. Automotive use cases include a road target tracking application, whose multi-modality requires using a PF or MPF [166]. The MPF is chosen as it allows reducing the number of particles for less computational effort. Similarly, [122] presents a MPF application for lane tracking, in which the achieved particle reduction compared to a pure PF enables executing the algorithm in real-time in an embedded system. *Adopted from [78]*.

According to (2.17), between spline function value and spline function control points there is a known linear relationship given by the blending or basis functions. Section 3.1 will state in (3.3) and (3.5) that this applies to both the value of a B-spline function and its derivatives as well. Therefore NWLS approximation leads to a mixed linear/nonlinear problem as long as there are target criteria that refer to the B-spline function or its derivatives directly. For being able to take into account this known linear relationship instead of having to estimate it, the iterative algorithm for NWLS approximation, NRBA, that is defined in Subsection 3.4.3, includes an MPF, which is described in Subsection 3.4.2.

### **Kernel adaptive filters**

The two previous subsections assumed that knowledge about the system state with respect to the measured input *u* and measured output y is available. Sometimes this does not apply or the system features are difficult to specify. In such

cases the unknown system function *f* with y = *f* (*x*) can be approximated by ˆ*f* . The approximation ˆ*f* is also called black-box model because it is created only from the data itself without system knowledge.

ˆ*f* can be determined with a kernel adaptive filter (KAF). Using a nonlinear mapping Φ(·), KAFs transform the input data *u* into a high-dimensional feature space, in which more degrees of freedom are available to solve the problem with a linear adaptive filter. Due to a property of reproducing kernel Hilbert spaces no costly explicit mapping is needed to compute inner products h·, ·i of the transformed data Φ(xi). Instead, inner product algorithms can be performed implicitly in feature space by replacing all inner products in the original problem by a kernel function κ [146]:

$$\kappa(\mathbf{x}\_j, \mathbf{x}\_m) = \langle \Phi(\mathbf{x}\_j), \Phi(\mathbf{x}\_m) \rangle \tag{2.20}$$

Often Gaussian kernels are used. These kernels differ from multi-dimensional PDFs of the standard normal distribution with variance σ <sup>2</sup> only in the missing normalization constant <sup>1</sup>/ σ √ 2π :

$$\kappa(\mathbf{x}\_j, \mathbf{x}\_m) = \exp(-\left\|\mathbf{x}\_j - \mathbf{x}\_m\right\|^2 / 2\sigma^2) \tag{2.21}$$

The nonlinear mapping ˆ*f* is calculated as a linear combination of kernels, each of which is weighted by its coefficient. For example, by transforming the RLS cost function into feature space, a nonlinear kernel RLS (KRLS) algorithm is derived. With most kernel-based methods the required memory increases with the processed data points. In contrast, the Fixed-Budget KRLS (FB-KRLS) algorithm can operate with constant memory by pruning the least significant data [180]. This enables long-term online applications.

In summary KAFs are nonlinear adaptive filters that result from applying kernel method concepts to linear adaptive filters. KAFs combine the capability of approximating any nonlinear function with the advantage that they only require solving a convex optimization problem instead of a nonlinear problem. For convex problems there are solvers that operate within a fixed time interval. This is important for real-time applications [146].

Subsection 4.7.2 will apply the FB-KRLS to represent the power train of the research vehicle. The model is updated constantly during vehicle operation.

# **3 Data approximation with B-spline functions**

This chapter deals with the general problem of recursive least squares approximation of data points with a B-spline function. First, Section 3.1 states a B-spline function definition in matrix form. Section 3.2 introduces a data set which will be approximated by a B-spline function. Section 3.3 addresses the case of linear least squares approximation and Section 3.4 the nonlinear case. Each of the two latter sections proposes a novel recursive algorithm and compares it with a well-known method for batch processing. Section 3.5 summarizes the scientific contribution.

## **3.1 B-spline function definition**

A B-spline function consists of several polynomial basis splines (B-splines), all of which have the same degree. A B-spline of degree *d* is piecewise defined using *d* + 1 polynomial functions of degree *d*. Figure 3.1 illustrates the composition of a cubic B-spline. The solidly drawn parts of the depicted polynomial functions *<sup>p</sup>*1, *<sup>p</sup>*2, *<sup>p</sup>*<sup>3</sup> and *<sup>p</sup>*<sup>4</sup> build a B-spline.

Let [κ1, κ2] denote a closed interval between and including <sup>κ</sup><sup>1</sup> and <sup>κ</sup><sup>2</sup> and let (κ1, κ2) denote an open interval between but not including <sup>κ</sup><sup>1</sup> and <sup>κ</sup>2.

In the interval [κ1, κ2), the B-spline is given by the first polynomial *<sup>p</sup>*1, in the interval [κ2, κ3) it is given by the second polynomial *<sup>p</sup>*<sup>2</sup> and so on. Outside the interval (κ1, κ5) the B-spline is zero. The dashed curves are only shown for

Figure 3.1: Piecewise definition of a cubic B-spline by the solid parts of four cubic polynomials <sup>p</sup>1, <sup>p</sup>2, . . ., <sup>p</sup>4. Vertical lines indicate the knots κ1, . . ., κ5.

better visualization of the polynomials but are not part of the B-spline. The interval borders <sup>κ</sup>1, κ2, . . ., κ<sup>5</sup> are also called *knots*. Their distances influence the shape of the polynomials and therefore the shape of the B-spline as well.

 A B-spline function is also piecewise defined. Its value is given by the weighted sum of *<sup>J</sup>* B-splines of degree *<sup>d</sup>*. κ with κ <sup>=</sup> (κ1, κ2, . . ., κJ+d+1) is the knot vector. Strictly increasing knot values (κ<sup>k</sup> < κk+1, *<sup>k</sup>* <sup>=</sup> <sup>1</sup>, <sup>2</sup>, . . ., *<sup>J</sup>* <sup>+</sup> *<sup>d</sup>*) are assumed. κ and *<sup>d</sup>* determine the number and shape of B-splines. The *<sup>j</sup>*-th B-spline *<sup>b</sup>*j(*s*), *<sup>j</sup>* <sup>=</sup> <sup>1</sup>, <sup>2</sup>, . . ., *<sup>J</sup>* is positive only for *<sup>s</sup>* <sup>∈</sup> (<sup>κ</sup> <sup>j</sup> , κj+d+1) and zero elsewhere [113, pp. 37-42].

The following definitions originate from [113, pp. 47-50, 65-70]: Let [κµ, κµ+1) be a spline interval and let µ denote the spline interval index with *<sup>d</sup>*+<sup>1</sup> <sup>≤</sup> µ <sup>≤</sup> *<sup>J</sup>*. For *<sup>s</sup>* <sup>∈</sup> [κµ, κµ+1), the B-splines *<sup>b</sup>*j(*s*), *<sup>j</sup>* <sup>=</sup> <sup>µ</sup> <sup>−</sup> *<sup>d</sup>*, . . ., µ can be nonzero. Their values for a specific *<sup>s</sup>* <sup>∈</sup> [κµ, κµ+1) can be summarized in the B-spline vector <sup>b</sup>µ,<sup>d</sup> (*s*) <sup>=</sup> (*b*µ−<sup>d</sup> (*s*), *<sup>b</sup>*µ−d+1(*s*), . . ., *<sup>b</sup>*µ (*s*)) <sup>∈</sup> <sup>R</sup> <sup>1</sup>×(d+1) which can be computed according to (3.1):

$$\mathbf{b}\_{\mu,d}(\mathbf{s}) = \underbrace{\mathbf{B}\_{\mu,1}(\mathbf{s})\,\mathbf{B}\_{\mu,2}(\mathbf{s})\,\dots\,\mathbf{B}\_{\mu,\delta}(\mathbf{s})\,\dots\,\mathbf{B}\_{\mu,\delta}(\mathbf{s})}\_{\in \mathbb{R}^{d\times(\delta+1)}}\dots\underbrace{\mathbf{B}\_{\mu,d}(\mathbf{s})}\_{\in \mathbb{R}^{d\times(d+1)}}\tag{3.1}$$

The B-spline matrix <sup>B</sup>µ,δ (*s*) <sup>∈</sup> <sup>R</sup> δ×(δ+1) is defined for each δ <sup>∈</sup> <sup>N</sup> with δ <sup>≤</sup> *<sup>d</sup>* and given by

$$\begin{array}{c} \mathbf{B}\_{\mu,\delta}(\mathbf{s}) = \\\\ \begin{bmatrix} \frac{\kappa\_{\mu+1} - s}{\kappa\_{\mu+1} - \kappa\_{\mu+1-\delta}} & \frac{s - \kappa\_{\mu+1-\delta}}{\kappa\_{\mu+1} - \kappa\_{\mu+1-\delta}} & 0 & \dots & 0\\ 0 & \frac{\kappa\_{\mu+2} - s}{\kappa\_{\mu+2} - \kappa\_{\mu+2-\delta}} & \frac{s - \kappa\_{\mu+2-\delta}}{\kappa\_{\mu+2} - \kappa\_{\mu+2-\delta}} & \dots & 0\\ \vdots & \vdots & \ddots & \vdots & \vdots\\ 0 & 0 & \dots & \frac{\kappa\_{\mu+\delta} - s}{\kappa\_{\mu+\delta} - \kappa\_{\mu}} & \frac{s - \kappa\_{\mu}}{\kappa\_{\mu+\delta} - \kappa\_{\mu}} \end{bmatrix} .\end{array} \tag{3.2}$$

The B-spline function *<sup>f</sup>* : D → <sup>R</sup> , *<sup>s</sup>* 7→ *<sup>f</sup>* (*s*) has the definition range <sup>D</sup> <sup>=</sup> [κd+1, κJ+1). For *<sup>s</sup>* <sup>∈</sup> [κµ, κµ+1), the B-spline function is given by

$$f(s) = \mathbf{b}\_{\mu,d}(s)\mathbf{x}\_{\mu,d} \tag{3.3}$$

with control point vector

$$\mathbf{x}\_{\mu,d} = \begin{pmatrix} \mathbf{x}\_{\mu-d}, \mathbf{x}\_{\mu-d+1}, \dots, \mathbf{x}\_{\mu} \end{pmatrix}^{\top}. \tag{3.4}$$

*Adopted from [78, 80]*. As stated, the spline types mentioned in Section 2.3 can all be defined using the same structure because they only differ in the geometry matrix M. Therefore (3.1) and (3.3) correspond to (2.16) and (2.17), which were obtained for an interpolating polynomial curve.

Figure 3.2 illustrates the construction of a B-spline function. Each cubic (*d*=3) B-spline *b*<sup>j</sup> is weighted with a corresponding control point *x* <sup>j</sup> , here *<sup>x</sup>*<sup>1</sup> <sup>=</sup> <sup>1</sup>.5, *<sup>x</sup>*<sup>4</sup> <sup>=</sup> <sup>0</sup>.<sup>5</sup> and all other control points equal one. The cubic B-spline function *f* (*s*) is given by the weighted sum of B-splines but only defined in the interval [κ4, κ7). Its shape is given by the solid part of the black curve.

The black dots denote the control points of the B-spline function. The horizontal position of the *j*-th point is determined by the value of *s*, for which the *j*-th B-spline reaches its maximum. The vertical position is identical to *x* <sup>j</sup> . In each interval [κk, κk+1), *<sup>k</sup>* <sup>=</sup> *<sup>d</sup>*+1, . . ., *<sup>K</sup>*, the B-spline function lies within the convex

Figure 3.2: Construction of a cubic B-spline function: Equidistant knots <sup>κ</sup>1, κ2, . . ., κ<sup>10</sup> (indicated by vertical straight lines) and J = 6 resulting B-splines bj , <sup>j</sup> <sup>=</sup> <sup>1</sup>, <sup>2</sup>, . . ., <sup>J</sup> weighted with control points <sup>x</sup>1, <sup>x</sup>2, . . ., <sup>x</sup>6. The black line indicates the sum of all weighted B-splines and its solid part is the B-spline function. Black dots represent the control points and the gray area is the convex hull formed by the first four control points. *Adopted from [80].*

hull of the relevant control points. For *<sup>s</sup>* <sup>∈</sup> [κ4, κ5), the four leftmost control points are the relevant ones. They form the convex hull indicated by the gray shaded area.

 A B-spline function of degree *d* is *d* − 1 times continuously differentiable. For *r* ∈ N0, the *r*-th derivative <sup>∂</sup> r ∂s <sup>r</sup> *f* (*s*) of the B-spline function with respect to *s* is given by

$$\frac{\partial^r}{\partial s^r}f(s) = \frac{\partial^r}{\partial s^r} \mathbf{b}\_{\mu,d}(s) \mathbf{x}\_{\mu,d} \tag{3.5}$$

with B-spline vector

$$\begin{cases} \frac{\partial^r}{\partial s^r} \mathbf{b}\_{\mu,d}(s) = \\\\ \frac{d!}{(d-r)!} \mathbf{B}\_{\mu,1}(s) \dots \mathbf{B}\_{\mu,d-r}(s) \mathbf{B}'\_{\mu,d-r+1} \dots \mathbf{B}'\_{\mu,d}, \text{ if } r \le d\\ \mathbf{0}\_{1 \times (d+1)}, & \text{otherwise.} \end{cases} \tag{3.6}$$

**0**1×(d+1) denotes a 1 × (*d* + 1) zero matrix. The matrix B 0 µ,δ <sup>∈</sup> <sup>R</sup> δ×(δ+1) is obtained by differentiating all entries in <sup>B</sup>µ,δ (*s*) with respect to *<sup>s</sup>*:

$$\mathbf{B}'\_{\mu,\delta} = \begin{bmatrix} \frac{-1}{\kappa\_{\mu+1} - \kappa\_{\mu+1-\delta}} & \frac{1}{\kappa\_{\mu+1} - \kappa\_{\mu+1-\delta}} & \dots & 0\\ \vdots & \ddots & \ddots & \vdots\\ 0 & \dots & \frac{-1}{\kappa\_{\mu+\delta} - \kappa\_{\mu}} & \frac{1}{\kappa\_{\mu+\delta} - \kappa\_{\mu}} \end{bmatrix} \tag{3.7}$$

*Adopted from [78, 80]*.

According to [161] the antiderivative or indefinite integral *F*(*s*) of *f* (*s*) with degree *d* Int = *d* + 1 is given by

$$F(s) = \int f(s)ds = \mathbf{b}^{\text{Int}}\_{\mu^{\text{Int}}, d^{\text{Int}}}(s) \mathbf{x}^{\text{Int}}\_{\mu^{\text{Int}}, d} \tag{3.8}$$

b Int uses the stricly increasing knot vector κ Int which differs from κ only by an additional knot at either end for the increased degree. µ Int equals µ <sup>+</sup> <sup>1</sup>. The control point vector x Int comprises one additional component than x and can be computed according to (3.9):

$$\mathbf{x}\_{1}^{\text{Int}} = \mathbf{0}, \ x\_{j+1}^{\text{Int}} = \frac{\kappa\_{j+d^{\text{Int}}+1} - \kappa\_{j}}{d^{\text{Int}}+1} \cdot \sum\_{i=1}^{j} \mathbf{x}\_{i}, \ j = 1, \ldots, J \tag{3.9}$$

The definite integral of *<sup>f</sup>* (*s*) over the interval [*s*1, *<sup>s</sup>*2] then reads

$$\int\_{s\_1}^{s\_2} f(s)ds = F(s\_2) - F(s\_1). \tag{3.10}$$

### **3.2 Structure of the data set**

 Section 3.3 and Section 3.4 will work with the set {(*s*p, <sup>y</sup><sup>p</sup> )}p=1,2,...,<sup>P</sup> of *<sup>P</sup>* data points. *<sup>p</sup>* denotes the time step, at which data point (*s*p, <sup>y</sup><sup>p</sup> ) is measured or observed.

*s*<sup>p</sup> is the value of the independent variable *s* at time step *p*. The vector of independent variables <sup>s</sup> is given by <sup>s</sup> <sup>=</sup> (*s*1, . . ., *<sup>s</sup>*p, . . ., *<sup>s</sup>*P) >.

<sup>y</sup><sup>p</sup> <sup>=</sup> (yp,1, <sup>y</sup>p,2, . . ., <sup>y</sup>p,v, . . ., <sup>y</sup>p,V<sup>p</sup> ) <sup>&</sup>gt; is a vector of *V*<sup>p</sup> measurements y that refer to *s*<sup>p</sup> and may come from different sensors. *V*<sup>p</sup> ∈ N can be different for each y<sup>p</sup> but it is assumed that *V*<sup>p</sup> *P* ∀*p*. The vector of all measurements y is composed as follows:

$$\mathbf{y}^{\top} = (\underbrace{\mathbf{y}\_{1,1}, \dots, \mathbf{y}\_{1,\mathbf{V}\_1}}\_{=:\mathbf{y}\_1^{\top}}, \dots, \mathbf{y}\_P^{\top}, \dots, \underbrace{\mathbf{y}\_{P,1}, \dots, \mathbf{y}\_{P,\mathbf{V}\_P}}\_{=:\mathbf{y}\_P^{\top}}) \tag{3.11}$$

*Adopted from [78, 80]*.

## **3.3 Methods for linear weighted least squares problems**

Subsection 3.3.1 describes the WLS approach followed by the KF algorithm in Subsection 3.3.2. Subsection 3.3.3 presents the RBA algorithm. Its effectiveness is demonstrated in comparison with the WLS solution in Subsection 3.3.4.

#### **3.3.1 Weighted least squares estimator**

 The linear weighted least squares (WLS) method estimates the constant state vector x ∈ R <sup>L</sup>×<sup>1</sup> of a linear system

$$\mathbf{y} = \mathbf{C}\mathbf{x} + \boldsymbol{\nu}.\tag{3.12}$$

y ∈ R N×1 is the vector of measurements from (3.11) and **C** denotes the measurement matrix that relates <sup>x</sup> to <sup>y</sup>. The measurement noise υ <sup>∈</sup> <sup>R</sup> N×1 is assumed to be an uncorrelated white noise process with mean zero. This implies that the covariance matrix of measurement noise **R** is a diagonal matrix and **R**i;<sup>i</sup> , *<sup>i</sup>* <sup>=</sup> <sup>1</sup>, . . ., *<sup>N</sup>* is the variance of measurement <sup>y</sup><sup>i</sup> which can differ from

the variances of other measurements [164]. The assumptions for {υ} can be generalized to a correlated noise process. This is termed generalized linear model [138, p. 143]. Then **R** is a positive definite matrix [20, p. 374].

The linear WLS estimate xˆ minimizes the sum of squared errors between the measurements y and the vector **C**x which are weighted with the reciprocals of the variances of the measurements:

$$\hat{\mathbf{x}} = \operatorname\*{arg\,min}\_{\mathbf{x}} (\mathbf{y} - \mathbf{C}\mathbf{x})^{\top} \mathbf{R}^{-1} (\mathbf{y} - \mathbf{C}\mathbf{x}) \tag{3.13}$$

The solution to optimization problem (3.13) is given by the closed-form estimator

$$\hat{\mathbf{x}} = (\mathbf{C}^\top \mathbf{R}^{-1} \mathbf{C})^{-1} \mathbf{C}^\top \mathbf{R}^{-1} \mathbf{y} \text{ [164]}.\tag{3.14}$$

From (3.3) follows that the value of a B-spline function is a linear combination of its control points. Therefore WLS can be used to determine the control points such that the function approximates the set of data points defined in Section 3.2. Then **C** is a P<sup>P</sup> p=1 *V*p × *J* matrix because y comprises P<sup>P</sup> p=1 *V*p scalar components <sup>y</sup>p,v, *<sup>p</sup>* <sup>=</sup> <sup>1</sup>, . . ., *<sup>P</sup>*, <sup>v</sup> <sup>=</sup> <sup>1</sup>, . . ., *<sup>V</sup>*<sup>p</sup> (c.f. (3.11)) and there are *<sup>J</sup>* B-splines. <sup>y</sup>p,<sup>v</sup> is the <sup>v</sup>˜-th component of <sup>y</sup> v˜ = Pp−<sup>1</sup> p˜ =1 *V*p˜ + v and provides information about <sup>∂</sup> r ∂s <sup>r</sup> *<sup>f</sup>* (*s*p) with *<sup>s</sup>*<sup>p</sup> <sup>∈</sup> [κµ, κµ+1) and an *<sup>r</sup>* <sup>∈</sup> <sup>N</sup>0. The <sup>v</sup>˜-th row of **<sup>C</sup>** is given by **<sup>C</sup>**v˜;1,...,<sup>J</sup> <sup>=</sup> <sup>c</sup> with

$$\mathbf{c} = \left( \mathbf{0}\_{1 \times (\mu - (d+1))}, \frac{\partial^r}{\partial s^r} \mathbf{b}\_{\mu, d}(\mathbf{s}\_p), \mathbf{0}\_{1 \times (J-\mu)} \right) \tag{3.15}$$

Herein **0**r×<sup>c</sup> denotes a *r* × *c* zero matrix. *Adopted from [80]*.

#### **3.3.2 Kalman filter**

 The Kalman filter (KF) is an established method for estimating the state of a dynamic system. Applications include tracking, navigation, sensor data fusion and process control [60, pp. 4-5]. The KF can be seen as a generalization of the RLS method [164, p. 129].

The linear KF estimates the state vector x<sup>p</sup> ∈ R <sup>L</sup>×<sup>1</sup> of a linear time-discrete system

$$\mathbf{x}\_p = \mathcal{H}\_p \mathbf{x}\_{p-1} + \mathcal{B}\_p \mathbf{u}\_p + \omega\_p \quad \quad \quad \quad \quad \quad (\text{State equation}) \qquad (3.16)$$

$$\mathbf{y}\_p = \mathcal{C}\_p \mathbf{x}\_p + \boldsymbol{\nu}\_p \tag{\text{Measurement equation}} \tag{3.17}$$

where *p* ∈ N denotes the time step. A<sup>p</sup> is the state transition matrix that relates xp−<sup>1</sup> to xp, u<sup>p</sup> ∈ R M×1 is an input signal vector with known influence on x<sup>p</sup> and B<sup>p</sup> is the input matrix that relates u<sup>p</sup> to xp. The vector of measurements is denoted by y<sup>p</sup> ∈ R N×1 and C<sup>p</sup> is the measurement matrix that relates x<sup>p</sup> to yp . <sup>ω</sup><sup>p</sup> <sup>∈</sup> <sup>R</sup> L×1 is the process noise with covariance matrix <sup>Q</sup><sup>p</sup> and <sup>υ</sup><sup>p</sup> <sup>∈</sup> <sup>R</sup> N×1 is the measurement noise with covariance matrix <sup>R</sup>p. Both {ωp} and {υp} are uncorrelated white noise processes with mean zero which implies that Q<sup>p</sup> and R<sup>p</sup> are diagonal matrices [164, p. 124].

The KF consists of a sequel of equations, which are computed for each time step and summarized in Algorithm 1, in which I denotes the identity matrix with appropriate dimensions. The KF performs a time update followed by a measurement update. During the time update, the state estimate is updated based on the knowledge about the system specified by (3.16). Both the a priori estimate xˆ − p and the covariance P − <sup>p</sup> of the a priori estimation error are calculated. During the measurement update, the Kalman gain K <sup>p</sup> is computed and used together with the information provided by measurement y<sup>p</sup> for the calculation of the corresponding a posteriori quantities xˆ + p and P + p [164, pp. 124-129]. KF generalizations for correlated or colored noise processes are stated in [164, pp. 183-193].

If the state vector <sup>x</sup><sup>p</sup> is constant, then <sup>A</sup><sup>p</sup> <sup>=</sup> <sup>I</sup>, <sup>ω</sup><sup>p</sup> <sup>=</sup> **<sup>0</sup>** and <sup>u</sup><sup>p</sup> <sup>=</sup> **<sup>0</sup>**, whereby **<sup>0</sup>** is the zero matrix with appropriate dimensions. In this case, the time update is redundant and the KF simplifies to the RLS algorithm [164, p. 129].

Data approximation by a polynomial using RLS is described in [164, pp. 92-93]. Due to the unbounded definition range of a polynomial, applying RLS saves computational effort without limiting the approximation compared to a KF.

Algorithm 1: Kalman filter. *Adopted from [80].* Input: xˆ + p−1 , P + p−1 , <sup>u</sup>p, <sup>y</sup><sup>p</sup> , <sup>A</sup>p, <sup>B</sup>p, <sup>C</sup>p, <sup>Q</sup>p, <sup>R</sup><sup>p</sup> /\* Time update \*/ <sup>1</sup> xˆ − <sup>p</sup> ← A<sup>p</sup> xˆ + p−1 + B<sup>p</sup> u<sup>p</sup> <sup>2</sup> P − <sup>p</sup> ← ApP + <sup>p</sup>−1A<sup>p</sup> <sup>&</sup>gt; + Q<sup>p</sup> /\* Measurement update \*/ <sup>3</sup> K <sup>p</sup> ← P − <sup>p</sup>C<sup>p</sup> <sup>&</sup>gt;(CpP − <sup>p</sup>C<sup>p</sup> <sup>&</sup>gt; + Rp) −1 <sup>4</sup> xˆ + <sup>p</sup> ← xˆ − <sup>p</sup> + K <sup>p</sup> (y<sup>p</sup> − C<sup>p</sup> xˆ − p ) <sup>5</sup> P + <sup>p</sup> ← (I − K <sup>p</sup>Cp)P − p (I − K <sup>p</sup>Cp) <sup>&</sup>gt; + K <sup>p</sup>RpK <sup>p</sup> > Output: xˆ + p , P + p

Data approximation by a B-spline based on the KF is proposed in [67, 104]. These approaches require that the state vector x<sup>p</sup> comprises all control points that are estimated during the whole approximation procedure. This means that xp, i.e. what is estimated, is constant and only the estimation xˆ <sup>p</sup> can change. Hence, RLS would suffice. Combined with the bounded definition range of a Bspline function, the missing ability to change which control points are estimated leads to a definition range of the approximation that is also bounded, constant and needs to be specified in advance.

In contrast to these approaches, the recursive B-spline approximation algorithm proposed in Subsection 3.3.3 takes advantage of the time update, that the KF provides, to shift the control points in xp. As a consequence, RBA can shift the definition range of the B-spline function to consider data points outside of the current definition range. *Adopted from [80]*.

#### **3.3.3 Recursive B-spline approximation algorithm**

 The recursive B-spline approximation (RBA) algorithm computes an approximating B-spline function *f* (*s*) of degree *d* for the set of data points from Section 3.2 iteratively using the KF. Algorithm 2 summarizes the calculations. *I* ∈ N denotes the constant number of spline intervals of *f* (*s*). The KF state

Figure 3.3: Changes of covariance matrix elements for d = 3 and I = 3. • indicates a large positive value, empty cells indicate zeros and ◦ denotes comparatively small values. *Adopted from [80].*

estimate xˆ <sup>p</sup> = (*x*ˆp<sup>1</sup> , *x*ˆp2 , . . ., *<sup>x</sup>*ˆp<sup>J</sup> ) <sup>&</sup>gt; comprises *J* = *d* + *I* components which are the estimated control points of *<sup>f</sup>* (*s*). The knot vector <sup>κ</sup> <sup>p</sup> <sup>=</sup> (κp<sup>1</sup> , κp<sup>2</sup> , . . ., κp<sup>K</sup> ) for time step *<sup>p</sup>* has to contain *<sup>K</sup>* <sup>=</sup> *<sup>J</sup>* <sup>+</sup> *<sup>d</sup>* <sup>+</sup> <sup>1</sup> knots. <sup>D</sup><sup>p</sup> <sup>=</sup> [κpd+<sup>1</sup> , κpJ+<sup>1</sup> ) is the definition range of *f* (*s*) at *p*.

#### **Initialization**

xˆ <sup>p</sup> is initialized with xˆ + 0 = *x*¯**1**J×1, where **1**J×<sup>1</sup> denotes a *J* × 1 matrix of ones and *<sup>x</sup>*¯ a scalar quantity of the magnitude of measurements <sup>y</sup>p,<sup>v</sup> that refer to ∂ r ∂s <sup>r</sup> *f* (*s*) with *r* = 0.

The covariance matrix of a posteriori estimation error P + is initialized with P + <sup>0</sup> = *p*¯I <sup>J</sup>×<sup>J</sup> , where I <sup>J</sup>×<sup>J</sup> is a *J* × *J* identity matrix. The scalar *p*¯ should be chosen large e.g. 10<sup>4</sup> because then xˆ <sup>p</sup> will quickly deviate from its initial value xˆ + 0 in such a way that *f* (*s*) approximates the data points. If the elements in P − p are small, this signals to the KF that the state estimate xˆ − p is very certain and therefore it will hardly be updated using the measurements. If the KF updates xˆ + p as intended, the elements in P + <sup>p</sup> become smaller as *p* increases.

In the long-run, P − p is strongly influenced by the covariance matrix of process noise Q<sup>p</sup> according to line 2 of Algorithm 1. If the elements in Q<sup>p</sup> are large, the elements in P − p remain large too. This can lead to volatile state estimates xˆ <sup>p</sup> that do not converge to a certain value. Then *f* (*s*) will not approximate the data points well. For that reason a very small positive value *q*¯ e.g. *q*¯ = 10−12 is chosen for Q<sup>p</sup> = *q*¯I <sup>J</sup>×<sup>J</sup> .

Algorithm 2: Recursive B-spline approximation. *Adopted from [80].*

```
Input: κ p−1, xˆ
                 +
                 p−1
                    , P
                       +
                       p−1
                          , Rp, sp, yp
                                     , κ¯ p, x¯, p¯, q¯
1 J ← length(xˆ
                 +
                 p−1
                    )
2 K ← length(κ p−1)
3 d ← K − J − 1
4 I ← J − d
5 σ ← 0
6 if sp ≥ κp−1J+1
                  then
7 if sp ≥ κp−1K
                     then
8 σ ← d + 1
9 else
10 σ such that sp ∈ [κp−1d+I+1+σ
                                       , κp−1d+I+2+σ
                                                   )
11 end
12 else if sp < κp−1d+1
                      then
13 if sp < κp−11
                    then
14 σ ← −(d + 1)
15 else
16 σ such that sp ∈ [κp−1d+1+σ
                                     , κp−1d+2+σ
                                                )
17 end
18 end
19 Ap ∈ R
           J×J
               from (3.19)
20 Qp ← q¯I J×J
21 if σ ≥ 0 then
22 κ p from (3.18)
23 up ←

              01×(J−σ)
                      , x¯11×σ
                              >
24 Qpm;m ← p¯, m = J − σ + 1, J − σ + 2, . . ., J
25 else
26 κ p from (3.25)
27 up ←

              x¯11×(−σ)
                      , 01×(J+σ)
                                >
28 Qpm;m ← p¯, m = 1, 2, . . ., −σ
29 end
30 µ such that sp ∈ [κpµ
                       , κpµ+1
                              )
31 Vp ← length(yp
                   )
32 Cp ∈ R
          Vp×J
               from (3.27)
33 [xˆ
     +
     p
      , P
         +
         p
          ] ← Algorithm 1(xˆ
                             +
                             p−1
                                 , P
                                    +
                                    p−1
                                       , up, yp
                                              , Ap, I J×J, Cp, Qp, Rp)
  Output: κ p, xˆ
                 +
                 p
                  , P
                     +
                     p
```
#### **Time update with shift operation**

RBA compares the knot vector <sup>κ</sup> <sup>p</sup>−<sup>1</sup> with *<sup>s</sup>*<sup>p</sup> in order to determine whether *s*<sup>p</sup> ∈ Dp−1. If necessary, a shift of Dp−<sup>1</sup> is performed during the time update of the KF such that *<sup>s</sup>*<sup>p</sup> ∈ Dp. The variable <sup>σ</sup> indicates the shift direction of <sup>D</sup>p−<sup>1</sup> and the number of positions by which elements are shifted. σ > <sup>0</sup> means a right shift of <sup>D</sup>p−1, σ < <sup>0</sup> a left shift of <sup>D</sup>p−<sup>1</sup> and for <sup>σ</sup> <sup>=</sup> <sup>0</sup> no shift is performed because *<sup>s</sup>*<sup>p</sup> ∈ Dp−1. Algorithm 2 computes <sup>σ</sup> from line 5 to line 18.

For example, assume *<sup>d</sup>* <sup>=</sup> <sup>3</sup>, *<sup>I</sup>* <sup>=</sup> <sup>3</sup> and <sup>κ</sup> <sup>p</sup>−<sup>1</sup> <sup>=</sup> (1, <sup>2</sup>, . . ., <sup>10</sup>), then <sup>D</sup>p−<sup>1</sup> <sup>=</sup> [4, <sup>7</sup>). If *<sup>s</sup>*<sup>p</sup> <sup>=</sup> <sup>8</sup>.5, two additional knots are needed to be able to perform a right shift by two elements (σ <sup>=</sup> <sup>2</sup>). <sup>11</sup> and <sup>12</sup> as additional knots give <sup>κ</sup> <sup>p</sup> <sup>=</sup> (3, <sup>4</sup>, . . ., <sup>12</sup>) and hence *<sup>s</sup>*<sup>p</sup> ∈ D<sup>p</sup> <sup>=</sup> [6, <sup>9</sup>).

Algorithm 2 distinguishes between σ <sup>≥</sup> <sup>0</sup> and σ < <sup>0</sup>. It assumes that the <sup>|</sup>σ<sup>|</sup> additional knots are the <sup>σ</sup> last components of the knot vector <sup>κ</sup>¯ <sup>p</sup> <sup>=</sup> (κ¯ p1 , κ¯ p2 , . . ., <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> ) in case of σ > <sup>0</sup> and that they are the <sup>−</sup>σ first components of <sup>κ</sup>¯ <sup>p</sup> in case of σ < <sup>0</sup>.

# **Case 1: Right shift of definition range (**σ <sup>≥</sup> <sup>0</sup>**)**

The new knot vector is

$$\begin{aligned} \kappa\_p &\leftarrow (\kappa\_{p-1\_{\sigma+1}}, \kappa\_{p-1\_{\sigma+2}}, \dots, \kappa\_{p-1\_K}, \\ &\bar{\kappa}\_{p\_{K-\sigma+1}}, \bar{\kappa}\_{p\_{K-\sigma+2}}, \dots, \bar{\kappa}\_{p\_K}). \end{aligned} \tag{3.18}$$

The elements in xˆ + p−1 are shifted by line 1 of Algorithm 1 using the state transition matrix

$$\mathcal{H}\_p \in \mathbb{R}^{J \times J} \text{ with } \mathcal{H}\_{p\_{g \cdot h}} = \begin{cases} 1, & \text{if } h = g + \sigma \\ 0, & \text{otherwise.} \end{cases} \tag{3.19}$$

xˆ − <sup>p</sup> ← A<sup>p</sup> xˆ + p−1 updates the old estimate xˆ + p−1 to

$$\hat{\mathfrak{X}}\_p^- = (\hat{\mathfrak{x}}\_{p-1\_{\sigma+1}}, \hat{\mathfrak{x}}\_{p-1\_{\sigma+2}}, \dots, \hat{\mathfrak{x}}\_{p-1\_{J-\sigma}}, \mathbf{0}\_{1 \times \sigma})^\top. \tag{3.20}$$

With the second part of the instruction (xˆ − <sup>p</sup> ← xˆ − <sup>p</sup> + B<sup>p</sup> up), input matrix B<sup>p</sup> = I <sup>J</sup>×<sup>J</sup> and input signal vector

$$\mathfrak{u}\_p = \left(\mathbf{0}\_{1 \times (J - \sigma)}, \vec{\mathbf{x}} \mathbf{1}\_{1 \times \sigma}\right)^\top \tag{3.21}$$

arbitrary initial estimates *x*¯ in xˆ − p can be obtained:

$$\hat{\mathfrak{x}}\_p^- = \left(\hat{\mathfrak{x}}\_{p-1\_{\sigma+1}}, \hat{\mathfrak{x}}\_{p-1\_{\sigma+2}}, \dots, \hat{\mathfrak{x}}\_{p-1\_{J-\sigma}}, \vec{\mathfrak{x}}\mathbf{1}\_{l\times\sigma}\right)^\top \tag{3.22}$$

P + p−1 is updated during the time update as well by line 2 of Algorithm 1. The first part of the instruction (P − <sup>p</sup> ← ApP + <sup>p</sup>−1A<sup>p</sup> <sup>&</sup>gt;) leads to P + <sup>p</sup>1, . . .,J−σ;1, . . .,J−σ <sup>←</sup> P − <sup>p</sup>−1σ+1, . . .,J;σ+1, . . .,<sup>J</sup> and all elements in the rows or columns *<sup>J</sup>* <sup>−</sup> σ <sup>+</sup> <sup>1</sup>, *<sup>J</sup>* <sup>−</sup> σ <sup>+</sup> <sup>2</sup>, . . ., *<sup>J</sup>* of <sup>P</sup> − p equal zero. Especially zeros on the main diagonal prevent that *<sup>x</sup>*ˆpJ−σ+<sup>1</sup> , *<sup>x</sup>*ˆpJ−σ+<sup>2</sup> , . . ., *<sup>x</sup>*ˆp<sup>J</sup> become different from the initial value *x*¯.

These zeros in P − p can be replaced with large values using the second part of the instruction, P − <sup>p</sup> ← P − <sup>p</sup> + Qp, with

$$\mathbf{Q}\_p \in \mathbb{R}^{J \times J} \text{ with } \mathbf{Q}\_{p\_{\bar{g}:h}} = \begin{cases} \bar{p}, & \text{if } h = g \land \mathcal{Q} \\ \bar{q}, & \text{if } h = g \land \neg \mathcal{Q} \\ 0, & \text{otherwise} \end{cases} \tag{3.23}$$

with

$$\mathcal{Q} = \begin{cases} h \ge J - \sigma + 1, & \text{if } \sigma \ge 0 \\ h \le -\sigma, & \text{if } \sigma < 0. \end{cases} \tag{3.24}$$

Figure 3.3 depicts different states of P + and P − , respectively. *d* = 3 and *I* = 3 lead to a 6×6 matrix P + . The diagonal values of P + 0 are initialized with a large positive value as depicted in (a). All other elements of P + 0 are initialized with zeros. After some data points in the second spline interval have been processed, different comparatively small values are in the submatrix P + 2:5,2:5 (b).

After data points that fall into the third spline interval have been taken into account, only the elements in the first row and column of P + still have their initialization values (c). If only the first part of the update instruction is executed during a right shift by one element (σ <sup>=</sup> <sup>1</sup>), the elements in the last row and column of P − become zero (d). With the second part of the instruction, these elements can be set to nonzero values. For <sup>Q</sup>6,<sup>6</sup> <sup>=</sup> • and all other elements of Q equal zero, matrix (e) is obtained.

# **Case 2: Left shift of definition range (**σ < <sup>0</sup>**)**

The new knot vector is

$$\kappa\_p \leftarrow \left( \vec{\kappa}\_{p\_1}, \vec{\kappa}\_{p\_2}, \dots, \vec{\kappa}\_{p\_{-qr}}, \kappa\_{p-1\_1}, \kappa\_{p-1\_2}, \dots, \kappa\_{p-1\_{K+\sigma}} \right) \tag{3.25}$$

and the input signal vector reads

$$\mathfrak{u}\_P = \left(\tilde{\mathbf{x}}\mathbf{1}\_{\mathbf{l}\times(-\sigma)}, \mathbf{0}\_{\mathbf{l}\times(J+\sigma)}\right)^\top. \tag{3.26}$$

#### **Effect of shift operation**

The shift operation is the distinguishing feature of RBA compared to the algorithms in [67, 104]. Due to the shift operation, the total number of control points that will be estimated during the application of RBA does neither have to be known in advance nor to be bounded. Using <sup>κ</sup> <sup>p</sup> and <sup>x</sup><sup>ˆ</sup> + p , the determined B-spline function *<sup>f</sup>* (*s*) can be evaluated at any *<sup>s</sup>* <sup>∈</sup> [κpd+<sup>1</sup> , κpd+I+<sup>1</sup> ).

By shifting the entries in κ <sup>p</sup>−1, <sup>x</sup><sup>ˆ</sup> + p−1 and P + p−1 , the required memory is held constant. In Algorithm 2 <sup>|</sup>σ<sup>|</sup> is chosen just large enough that the current data point can be taken into account during the measurement update. The reason for this is that the shift operation comes at the cost that parts of an already

computed approximation are forgotten unless the values of κ <sup>p</sup>−1, <sup>x</sup><sup>ˆ</sup> + p−1 and P + p−1 are stored separately from Algorithm 2 before they are removed from the vectors or matrix, respectively.

#### **Measurement update**

During the measurement update, the information provided by (*s*p, <sup>y</sup><sup>p</sup> ) is used to update *f* (*s*). With the covariance matrix of measurement noise Rp, different components of the measurement vector y<sup>p</sup> can be weighted relatively to each other. R<sup>p</sup> ∈ R <sup>V</sup>p×V<sup>p</sup> is a diagonal matrix with positive elements on its diagonal. The smaller an entry Rpv;<sup>v</sup> is, the greater is the effect of the v-th component of the measurement error (y<sup>p</sup> − C<sup>p</sup> xˆ − p ) on xˆ + p .

The measurement matrix <sup>C</sup><sup>p</sup> is a *<sup>V</sup>*<sup>p</sup> <sup>×</sup> *<sup>J</sup>* matrix. <sup>y</sup>p,<sup>v</sup> is the <sup>v</sup>-th component of y<sup>p</sup> and provides information about <sup>∂</sup> r ∂s <sup>r</sup> *<sup>f</sup>* (*s*p) with *<sup>s</sup>*<sup>p</sup> <sup>∈</sup> [κµ, κµ+1) and an *r* ∈ N0. The v-th row of C<sup>p</sup> is given by

$$\mathcal{C}\_{P\_{\mathbf{V};\mathbf{l},\ldots,\mathbf{J}}} = \mathbf{c} \tag{3.27}$$

with <sup>c</sup> from (3.15). According to <sup>C</sup>p, (*s*p, <sup>y</sup><sup>p</sup> ) influences only the estimates *x*ˆpµ−<sup>d</sup> , *<sup>x</sup>*ˆpµ−d+<sup>1</sup> , . . ., *<sup>x</sup>*ˆpµ . However, other estimates can still be updated by the KF using the information stored in P − p . *Adopted from [80]*.

### **3.3.4 Effectiveness of recursive B-spline approximation**

 A numerical experiment in [80] demonstrates the effectiveness of Algorithm 2 in comparison with the corresponding WLS solution.

The data set {(*s*p, <sup>y</sup><sup>p</sup> )}p=1,2,...,<sup>P</sup> from Section 3.2 with *<sup>P</sup>* <sup>=</sup> <sup>5000</sup> is chosen, whereby

$$s\_p = 0.01 + 0.02(p - 1),\tag{3.28}$$

$$\mathbf{y}\_{p,1} = \begin{cases} 20, \text{if } 30 \le s\_p < 70 \\ 10, \text{otherwise} \end{cases} \quad \text{and} \tag{3.29}$$

$$\mathbf{y}\_{p,2} = \mathbf{y}\_{p,3} = \mathbf{0} \,\forall p. \tag{3.30}$$

A cubic (*d* = 3) B-spline function *f* (*s*) approximates the data set, whereby it is assumed that the measurements <sup>y</sup>p,<sup>2</sup> refer to the slope <sup>∂</sup> ∂s *f* (*s*) of the B-spline function and the measurements <sup>y</sup>p,<sup>3</sup> to the curvature <sup>∂</sup> 2 ∂s 2 *f* (*s*). The reciprocals of the relative weights between the different target criteria are specified by the diagonal measurement covariance matrix R<sup>p</sup> ∈ R <sup>3</sup>×<sup>3</sup> with Rp1;1 = 1, Rp2;2 = 10−<sup>2</sup> and Rp3;3 = 10−<sup>3</sup> . The diagonal measurement covariance matrix of WLS **R** ∈ R <sup>3</sup>P×3<sup>P</sup> has analogous values:

$$\begin{aligned} \mathbf{R}\_{i+1;i+1} &= 1, \; \mathbf{R}\_{i+2;i+2} = 10^{-2}, \; \mathbf{R}\_{i+3;i+3} = 10^{-3}, \\\mathbf{R}\_{i} &= \mathbf{3}(r-1), \; r = 1, 2, \; \dots, P \end{aligned} \tag{3.31}$$

The chosen weighting helps to prevent overshoots and oscillations of *f* (*s*) and leads to a B-spline function that smooths the jumps of <sup>y</sup>p,<sup>1</sup> in the data set. The parameter values are *q*¯ = 10−<sup>12</sup> , *x*¯ = 0 and *p*¯ = 10<sup>4</sup> .

In order to investigate the effect of the choice of *I*, four runs of RBA with *<sup>I</sup>* <sup>=</sup> <sup>1</sup>, <sup>3</sup>, <sup>7</sup> and <sup>20</sup>, respectively, are performed. <sup>κ</sup><sup>0</sup> <sup>=</sup> (−15, <sup>10</sup>, <sup>15</sup>, <sup>20</sup>) is used for *<sup>I</sup>* <sup>=</sup> <sup>1</sup>, <sup>κ</sup><sup>0</sup> <sup>=</sup> (−15, <sup>10</sup>, . . ., <sup>30</sup>) for *<sup>I</sup>* <sup>=</sup> <sup>3</sup>, <sup>κ</sup><sup>0</sup> <sup>=</sup> (−15, <sup>10</sup>, . . ., <sup>50</sup>) for *<sup>I</sup>* <sup>=</sup> <sup>7</sup>, and <sup>κ</sup><sup>0</sup> <sup>=</sup> (−15, <sup>10</sup>, . . ., <sup>115</sup>) for *<sup>I</sup>* <sup>=</sup> <sup>20</sup>. For *<sup>I</sup>* <sup>=</sup> <sup>20</sup> the resulting <sup>D</sup><sup>0</sup> comprises all *<sup>s</sup>*<sup>p</sup> of the data set and therefore no shift operation is needed. For *<sup>I</sup>* <sup>=</sup> <sup>1</sup>, <sup>3</sup> and 7, RBA has to perform several right shifts by one element in order to be able to process all data points. For each shift operation, an additional knot <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> has to be defined in the vector <sup>κ</sup>¯ <sup>p</sup>. <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> <sup>=</sup> <sup>25</sup>, <sup>30</sup>, . . ., <sup>115</sup> is chosen for *<sup>I</sup>* <sup>=</sup> <sup>1</sup>, <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> <sup>=</sup> <sup>35</sup>, <sup>40</sup>, . . ., <sup>115</sup> for *<sup>I</sup>* <sup>=</sup> <sup>3</sup> and <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> <sup>=</sup> <sup>55</sup>, <sup>60</sup>, . . ., <sup>115</sup> for *<sup>I</sup>* <sup>=</sup> <sup>7</sup>. For

Figure 3.4: Effectiveness of RBA: <sup>40</sup> of the <sup>5000</sup> data points (sp, <sup>y</sup>p,1) (black dots) and knots <sup>0</sup>, <sup>5</sup>, . . ., <sup>100</sup> (vertical dashed lines) are shown as well as the B-spline function <sup>f</sup> (s) determined by WLS and RBA with <sup>I</sup> <sup>=</sup> <sup>1</sup>, <sup>3</sup>, <sup>7</sup> and <sup>20</sup>. Arrows visualize the definition range of <sup>f</sup> (s) while data points in the interval [95, <sup>100</sup>) are processed. *Adopted from [80].*

evaluation purposes *x*ˆp−1<sup>1</sup> and <sup>κ</sup>p−1<sup>1</sup> are stored separately from RBA before a shift operation is performed.

Figure 3.4 displays the results. As both the data set and knot vector are symmetrical to a vertical straight line through *s* = 50, the WLS solution is symmetrical as well. The RBA solution converges to the WLS solution as *I* increases. For *I* = 1 and *I* = 3, the resulting B-spline function is asymmetrical with respect to a straight vertical line through *s* = 50. For *I* = 7, RBA provides almost the same result as for *I* = 20. Consequently, in this example *I* can be reduced from 20 to 7 without noticeably worsening the quality of the approximation. Lowering *I* leads to less computational effort in the KF because P − p , P + p and Q<sup>p</sup> are (*d* + *I*) × (*d* + *I*) matrices and therefore the asymptotic time complexity of each individual iteration of Algorithm 1 is O((*d*+*I*) 3 ) if the standard method for matrix multiplication is used [25, 97]. Under the same conditions both RLS and the known methods of other researchers based on the KF need 20 × 20 matrices because shift operations are not possible.

Due to the large possible savings in computational effort, RBA is also beneficial in offline applications with known finite data set and therefore a definition range that is known in advance.

In further experiments in [80] <sup>κ</sup><sup>0</sup> is chosen such that the first data points lie in the rightmost spline interval and runs with *I* = 3, *I* = 7 and *I* = 20 are performed. The deviations of the resulting control point vectors from the those derived when the first data points are in the leftmost interval are close to the machine accuracy indicating that the effect of the shift operation on the approximation result is negligible. *Adopted from [80]*.

# **3.4 Methods for nonlinear weighted least squares problems**

Subsection 3.4.1 describes the Levenberg-Marquardt (LM) algorithm followed by the marginalized particle filter in Subsection 3.4.2. Subsection 3.4.3 presents the nonlinear recursive B-spline approximation algorithm for nonlinear weighted least squares problems. Its effectiveness is demonstrated in comparison with the LM solution in Subsection 3.4.4.

#### **3.4.1 Levenberg-Marquardt algorithm**

Consider the problem

$$
\hat{\mathbf{x}} = \operatorname\*{arg\,min}\_{\mathbf{x}} \mathbf{e}^{\top} \mathbf{e} \tag{3.32}
$$

with the error function **e** given by

$$\mathbf{e} = \sqrt{\mathbf{R}^{-1}} \cdot \left(\mathbf{y} - \phi(s, \mathbf{x})\right). \tag{3.33}$$

s denotes the vector of independent variables, y the vector of measurements and **R** the covariance matrix of measurement noise. In (3.33) the difference between <sup>y</sup> and φ is weighted with the square root of the reciprocal of the covariance matrix of measurement noise **<sup>R</sup>**. φ is a function whose values depend nonlinearly on its control points which are summarized in the control point vector x. Due to this nonlinear relationship (3.32) is a NWLS problem.

The Levenberg-Marquardt (LM) algorithm solves this problem iteratively. xˆ i denotes the known approximation of the solution xˆ of (3.32) in iteration *i*. LM approximates the error function linearly using

$$\mathbf{e}(\hat{\mathbf{x}}) = \mathbf{e}(\hat{\mathbf{x}}^i) + \mathbf{e}'(\hat{\mathbf{x}}^i)(\hat{\mathbf{x}} - \hat{\mathbf{x}}^i) \tag{3.34}$$

and updates xˆ i according to

$$
\hat{\mathfrak{x}}^{i+1} = \hat{\mathfrak{x}}^i + \hat{\mathfrak{s}}^i \tag{3.35}
$$

with correction step size δ. The optimal step size δ<sup>ˆ</sup> is derived by solving the linear LS problem

$$\boldsymbol{\delta}^{i} = \underset{\boldsymbol{\delta}^{i}}{\text{arg min}} \|\mathbf{e}^{\prime}(\hat{\mathbf{x}}^{i})\boldsymbol{\delta} + \mathbf{e}(\hat{\mathbf{x}}^{i})\|\_{2}^{2} + \lambda \|\boldsymbol{\delta}^{i}\|\_{2}^{2} \tag{3.36}$$

which includes a damping parameter λ > <sup>0</sup>. The solution δ<sup>ˆ</sup> i to (3.36) with

$$\boldsymbol{\delta}^{i} = -\left[\mathbf{e}'(\hat{\mathbf{x}}^{i})^{\top}\mathbf{e}'(\hat{\mathbf{x}}^{i}) + \lambda^{2}I\right]^{-1}\mathbf{e}'(\hat{\mathbf{x}}^{i})^{\top}\mathbf{e}(\hat{\mathbf{x}}^{i})\tag{3.37}$$

is determined by setting the first derivative of its optimization function with respect to δ to zero. A too large δ can be avoided by choosing λ appropriately because of

$$\|\delta^{\hat{t}}\|\_{2} \le \frac{\|\mathbf{e}(\hat{\mathbf{x}}^{\hat{t}})\|\_{2}}{\lambda}.\tag{3.38}$$
 
$$\therefore x$$

The LM algorithm terminates if

$$\|\|\mathbf{e}'(\hat{\mathbf{x}}^i)^\top \mathbf{e}(\hat{\mathbf{x}}^i)\|\|\_2 \le \epsilon \tag{3.39}$$

with a specified tolerance . For convergence λ needs to be sufficiently large. However, a large λ results in a small correction step size and slow convergence if the solution is still far away from the optimum. Therefore λ is controlled using heuristic criteria [35, pp. 222-224].

The computational effort of a single LM iteration roughly equals the effort of the WLS method. The total computational effort of LM depends on the number of performed iterations and is given by <sup>O</sup>( −2 ) [175].

#### **3.4.2 Marginalized particle filter**

 The marginalized particle filter (MPF) is an iterative algorithm for estimating the unknown state vector x<sup>p</sup> of a system at time step *p* ∈ N.

In the MPF, x<sup>p</sup> is subdivided into x<sup>p</sup> = x L p > , x N p > > , whereby the KF optimally estimates the linear state vector x L p and a PF estimates the nonlinear state vector x N p . Exploiting linear substructures allows for better estimates and a reduction of the computational effort. Therefore, the MPF is beneficial for mixed linear/nonlinear state-space models [155]. Due to Equations (3.3) and (3.5), linear substructures will occur in approximation problems as long as there are target criteria that refer to the value of the B-spline function or its derivatives directly.

MPF algorithms for several state-space models can be found in [155] along with a MATLAB example that can be downloaded from [156]. An equivalent but new formulation of the MPF that allows for reused, efficient, and wellstudied implementations of standard filtering components is stated in [70].

For a NWLS approximation, the following state-space model derived from [70] is applied:

$$\mathbf{x}\_{p+1}^{N} = \mathcal{R}\_p^N \mathbf{x}\_p^N + \boldsymbol{\omega}\_p^N + \boldsymbol{\upmu}\_p^N \qquad \text{(Nonlinear state equation)} \tag{3.40}$$

$$\begin{aligned} \mathbf{x}\_{p+1}^{L} &= \mathcal{H}\_p^L \mathbf{x}\_p^L + \boldsymbol{\omega}\_p^L + \mathbf{u}\_p^L \\ \mathbf{y}\_p &= \mathbf{C} \mathbf{x}\_p^L + c \left(\mathbf{x}\_p^N\right) + \boldsymbol{\nu}\_p \end{aligned} \qquad \text{(Linear state equation)} \qquad (3.41)$$

The superscripts <sup>L</sup> and <sup>N</sup> indicate that the corresponding quantity refers to linear or nonlinear state variables, respectively. A<sup>p</sup> denotes the state transition matrix, u<sup>p</sup> is the known input vector, y<sup>p</sup> is the vector of measurements, C<sup>p</sup>

is the measurement matrix, and *c* is the nonlinear measurement function that depends on x N p . ωL <sup>p</sup> denotes the process noise of the linear state vector with a covariance matrix Q L p , ωN p is the process noise of the nonlinear state vector with a covariance matrix Q N p , and <sup>υ</sup><sup>p</sup> is the measurement noise with a covariance matrix Rp. The model of the conditionally linear subsystem in the KF has the state vector ξ > , x L > > , whereby ξ describes the linear dynamics of <sup>x</sup> N :

$$\begin{aligned} \begin{pmatrix} \boldsymbol{\xi}\_{p+1} \\ \boldsymbol{\pi}\_{p+1}^{L} \end{pmatrix} &= \begin{pmatrix} \mathbf{0} & \boldsymbol{\mathcal{H}}\_{p}^{N} \\ \mathbf{0} & \boldsymbol{\mathcal{H}}\_{p}^{L} \end{pmatrix} \begin{pmatrix} \boldsymbol{\xi}\_{p} \\ \mathbf{x}\_{p}^{L} \end{pmatrix} + \begin{pmatrix} \boldsymbol{\mu}\_{p}^{N} \\ \boldsymbol{\mu}\_{p}^{L} \end{pmatrix} + \begin{pmatrix} \boldsymbol{\omega}\_{p}^{N} \\ \boldsymbol{\omega}\_{p}^{L} \end{pmatrix} \\ \mathbf{y}\_{p} &= \begin{pmatrix} \mathbf{0} & \boldsymbol{\mathcal{C}}\_{p} \end{pmatrix} \begin{pmatrix} \boldsymbol{\xi}\_{p} \\ \boldsymbol{\mathcal{x}}\_{p}^{L} \end{pmatrix} + c \begin{pmatrix} \mathbf{x}\_{p}^{L} \end{pmatrix} + \boldsymbol{\nu}\_{p} \end{aligned} \tag{3.43}$$

The covariance matrix of process noise is \* . , Q N <sup>p</sup> **0 0** Q L p + / - , and **0** denotes a zero matrix with a suitable size.

A PF with the model

$$\begin{aligned} \mathbf{x}\_{p+1}^{N} &= \bar{\boldsymbol{\omega}}\_{p}^{N} \\ \mathbf{y}\_{p} &= \bar{\boldsymbol{\nu}}\_{p} \end{aligned} \tag{3.44}$$

deals with the remaining nonlinear effects. The noise depends on the estimates indicated by ˆ from the conditionally linear model:

$$\begin{aligned} \bar{\boldsymbol{\omega}}\_p^N &\sim \mathcal{N}\left(\hat{\boldsymbol{\xi}}\_p, \mathcal{P}\_p^{\xi, -}\right) \\ \bar{\boldsymbol{\nu}}\_p &\sim \mathcal{N}\left(\boldsymbol{c}\left(\mathbf{x}\_p^N\right) + \mathbf{C}\_P\left(\mathbf{x}\_p^N\right)\hat{\mathbf{x}}^{L, -}, \mathbf{S}\_p\right) \end{aligned} \tag{3.45}$$

with

$$\mathbf{S}\_p = \mathbf{C}\_p \mathbf{\mathcal{P}}\_p^{L,-} \mathbf{C}\_p^\top + \mathbf{\mathcal{R}}\_p \tag{3.46}$$

where the superscript <sup>−</sup> refers to a priori quantities that are computed in the time update, which is based on the state of (3.40) and (3.41). In contrast, <sup>+</sup>

denotes a posteriori quantities that are calculated in the following measurement update based on the measurement of (3.42).

P L,− p and P ξ,− <sup>p</sup> are the covariance matrices of the estimation errors that belong to xˆ L p and ξ<sup>ˆ</sup> p , respectively.

The PF uses multiple state estimates, called *particles*, simultaneously. The superscript <sup>p</sup> with *<sup>p</sup>* <sup>=</sup> <sup>1</sup>, . . ., *<sup>P</sup>* is the particle index and *<sup>P</sup>* is the particle count. In general, a KF is used for each particle. In the chosen state-space model, however, A<sup>L</sup> p , A<sup>N</sup> p , Q L p , and Q N p are independent of x L p and x N p . This implies that P L,− p and P ξ,− <sup>p</sup> are identical for all KFs, which reduces the computational effort substantially [70, 155].

Algorithm 3 states the equations for one MPF iteration and was derived from [70, 155]. For an implementation in MATLAB, the example from [156] was adapted. Note that in Algorithm 3 the measurement update of the previous time step *p* − 1 occurs before the time update for the current time step *p*, similar to the algorithm in [6].

In line 4 of Algorithm 3, linear particles are resampled according to their corresponding normalized importance weights. After resampling, particles with a low measurement error occur more frequently in the set of particles. Subsequently, all particles xˆ <sup>L</sup>,+,<sup>p</sup> p−1 are aggregated in line 5 to a single estimate xˆ + p−1 by calculating their mean.

After both KF and PF have been time updated, the KF is adjusted based on the PF estimates in a mixing step with the cross-covariances of the estimation errors, P ξL,<sup>−</sup> <sup>p</sup> and P <sup>L</sup>ξ,<sup>−</sup> <sup>p</sup> .

In the new formulation from [70], resampling occurs after the measurement update of both PF and KF. Therefore, the quantities computed for the measurement update of the PF can be reused for the KF measurement update. In particular, each particle is only evaluated once in line 1 of each MPF iteration instead of twice as with the previous formulation in [155]. *Adopted from [78]*.

Algorithm 3: Marginalized particle filter derived from [70, 155]. *Adopted from [78].*

\*  $\left. \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \right.$  \*  $\left. \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt}$  \*  $\left. \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt^{p}} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \right.$   $\frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt}$  \*  $\left. \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt} \frac{d^{p}}{dt}$ 

### **3.4.3 Nonlinear recursive B-spline approximation algorithm**

 Nonlinear recursive B-spline approximation (NRBA) adapts a B-spline function *f* (*s*) with degree *d* iteratively to the data set from Section 3.2. Algorithm 4 states the instructions for one iteration of NRBA, which is based on the MPF.

In each iteration *p*, NRBA modifies *f* in *I* ∈ N consecutive spline intervals. Each linear particle xˆ L,p <sup>p</sup> = (*x*ˆ L p1 , *x*ˆ L p2 , . . ., *<sup>x</sup>*<sup>ˆ</sup> L pJ ) <sup>&</sup>gt; and each nonlinear particle xˆ N,p <sup>p</sup> = (*x*ˆ N p1 , *x*ˆ N p2 , . . ., *<sup>x</sup>*<sup>ˆ</sup> N pJ ) <sup>&</sup>gt; contains estimates for *J* = *d* + *I* control points of *<sup>f</sup>* . <sup>κ</sup> <sup>p</sup> <sup>=</sup> (κp<sup>1</sup> , κp<sup>2</sup> , . . ., κp<sup>K</sup> ) denotes the knot vector comprising *K* = *J* + *d* + 1 knots. The resulting definition range <sup>D</sup><sup>p</sup> of *<sup>f</sup>* is given by <sup>D</sup><sup>p</sup> <sup>=</sup> [κpd+<sup>1</sup> , κpJ+<sup>1</sup> ). NRBA checks if *s*<sup>p</sup> is in the definition range of the previous time step, Dp−1. If not, Dp−<sup>1</sup> needs to be shifted such that *s*<sup>p</sup> ∈ Dp. A shift can be conducted in the MPF time update. The result of the time update is the a priori estimate xˆ − p . In the following measurement update, *s*<sup>p</sup> is needed again to compute the measurement matrix Cp, and then, to take into account y<sup>p</sup> . The result of the measurement update is the a posteriori estimate xˆ + p .

Figure 3.5 depicts the allocation of available data points and computed estimates xˆ to KF iterations in RBA versus MPF iterations in NRBA. The arrows indicate the needed information for computing the estimates. The KF is initialized with xˆ + 0 and conducts in each iteration a time update first and then a measurement update. Therefore, *P* iterations are required for *P* data points. In contrast, the MPF performs the measurement update first and is initialized with xˆ − 0 . Therefore, y<sup>p</sup> has to be stored and *s*p, *s*p+1, and y<sup>p</sup> have to be provided for iteration *p*+1. Hence, in order to take into account all data points, an additional iteration compared to the KF is needed. By definition, (*s*1, <sup>y</sup><sup>1</sup> ) is used for computing xˆ + 0 and *s*<sup>n</sup> for xˆ − n+1 as indicated by the dashed arrows.

Algorithm 4: Nonlin. recurs. B-spline approximation. *Adopted from [78].*

Input: κ <sup>p</sup>−1, <sup>x</sup><sup>ˆ</sup> <sup>L</sup>,−,<sup>p</sup> p−1 , xˆ <sup>N</sup>,−,<sup>p</sup> p−1 , xˆ + p−2 , P L,− p−1 , <sup>R</sup>p−1, *<sup>s</sup>*p, *<sup>s</sup>*p−1, <sup>y</sup><sup>p</sup> , <sup>y</sup>p−<sup>1</sup> , κ¯ <sup>p</sup>, *<sup>p</sup>*¯, *q*¯ L , *q*¯ N, *c* <sup>1</sup> *J* ← length xˆ <sup>L</sup>,+,<sup>p</sup> p−1 ; *K* ← length <sup>κ</sup> <sup>p</sup>−<sup>1</sup> ; *d* ← *K* − *J* − 1; *I* ← *J* − *d* /\* Quantities for MPF measurement update \*/ <sup>2</sup> Compute <sup>µ</sup> such that *<sup>s</sup>*p−<sup>1</sup> <sup>∈</sup> [κp−1µ , κp−1µ+<sup>1</sup> ); *V*p−<sup>1</sup> ← length(yp−<sup>1</sup> ) <sup>3</sup> Cp−<sup>1</sup> ∈ R Vp−1×J from (3.47) /\* Quantities for MPF time update \*/ <sup>4</sup> σ <sup>←</sup> <sup>0</sup> <sup>5</sup> if *<sup>s</sup>*<sup>p</sup> <sup>≥</sup> <sup>κ</sup>p−1J+<sup>1</sup> then <sup>6</sup> if *<sup>s</sup>*<sup>p</sup> <sup>≥</sup> <sup>κ</sup>p−1<sup>K</sup> then <sup>7</sup> σ <sup>←</sup> *<sup>d</sup>* <sup>+</sup> <sup>1</sup> 8 else <sup>9</sup> Compute <sup>σ</sup> such that *<sup>s</sup>*<sup>p</sup> <sup>∈</sup> [κp−1d+I+1+σ , κp−1d+I+2+σ ) 10 end <sup>11</sup> else if *<sup>s</sup>*<sup>p</sup> < κp−1d+<sup>1</sup> then <sup>12</sup> if *<sup>s</sup>*<sup>p</sup> < κp−1<sup>1</sup> then <sup>13</sup> σ ← −(*<sup>d</sup>* <sup>+</sup> <sup>1</sup>) 14 else <sup>15</sup> Compute <sup>σ</sup> such that *<sup>s</sup>*<sup>p</sup> <sup>∈</sup> [κp−1d+1+σ , κp−1d+2+σ ) <sup>16</sup> end <sup>17</sup> end <sup>18</sup> if σ <sup>≥</sup> <sup>0</sup> then <sup>19</sup> *x*¯ ← last element of xˆ + p−2 <sup>20</sup> κ <sup>p</sup>, <sup>u</sup> L p , u N p from (3.18), (3.49) and (3.53) <sup>21</sup> else <sup>22</sup> *x*¯ ← first element of xˆ + p−2 <sup>23</sup> κ <sup>p</sup>, <sup>u</sup> L p , u N p from (3.18), (3.55) and (3.56) <sup>24</sup> end <sup>25</sup> Compute <sup>µ</sup> such that *<sup>s</sup>*<sup>p</sup> <sup>∈</sup> [κpµ , κpµ+<sup>1</sup> ) <sup>26</sup> A<sup>L</sup> p , Q L p , A<sup>N</sup> p and Q N p from (3.48), (3.51), (3.52) and (3.54) 27 f P L,− p , xˆ + p−1 , xˆ <sup>L</sup>,−,<sup>p</sup> <sup>p</sup> , <sup>x</sup><sup>ˆ</sup> <sup>N</sup>,−,<sup>p</sup> p g ← Algorithm 3 A<sup>L</sup> p , <sup>A</sup><sup>N</sup> p , <sup>C</sup>p−1, *<sup>c</sup>*, <sup>P</sup> L,− p−1 , Q L p , Q N p , <sup>R</sup>p−1, <sup>u</sup> L p , u N p , xˆ <sup>L</sup>,−,<sup>p</sup> p−1 , xˆ <sup>N</sup>,−,<sup>p</sup> p−1 , <sup>y</sup>p−<sup>1</sup> Output: κ <sup>p</sup>, <sup>x</sup><sup>ˆ</sup> + p−1 , xˆ <sup>L</sup>,−,<sup>p</sup> <sup>p</sup> , <sup>x</sup><sup>ˆ</sup> <sup>N</sup>,−,<sup>p</sup> <sup>p</sup> , <sup>P</sup> L,− p

#### **Initialization**

Each linear particle xˆ <sup>L</sup>,−,<sup>p</sup> 0 is initialized with xˆ <sup>L</sup>,−,<sup>p</sup> 0 = *x*¯ Init**1**J×1, and each nonlinear particle xˆ <sup>N</sup>,−,<sup>p</sup> 0 is initialized with

$$
\hat{\mathbf{x}}\_0^{N,-,p} = \bar{\mathbf{x}}^{\text{Int}} \mathbf{1}\_{J \times 1} + \text{chol} \left( \bar{p} I\_{J \times J} \right) \cdot \mathbf{r} \mathbf{n} \mathbf{d}\_{J \times 1}.
$$

Hereby, **1**J×<sup>1</sup> is a *J* × 1 matrix of ones and *x*¯ Init indicates an initial value equal to the scalar measurement <sup>y</sup>1,<sup>v</sup> referring to *<sup>f</sup>* . chol(·) computes the Cholesky factorization, and rndJ×<sup>1</sup> is a *J* × 1 vector of random values drawn from the standard normal distribution. The covariance matrix of a priori estimation error of linear states, P L,− , is initialized with P L,− 0 = *p*¯I <sup>J</sup>×<sup>J</sup> . I <sup>J</sup>×<sup>J</sup> denotes a *J* × *J* identity matrix.

The large scalar value *p*¯ causes xˆ <sup>p</sup> to quickly change such that *f* adapts to the data. Provided that the values in Q L p are small, the values in P L,− <sup>p</sup> decrease as *p* grows according to line 8 of Algorithm 3. Small elements in P L,− p correspond to certain estimates. Therefore, the particles xˆ <sup>L</sup>,−,<sup>p</sup> <sup>p</sup> and xˆ <sup>N</sup>,−,<sup>p</sup> <sup>p</sup> are slower to be updated using measurements such that *f* converges. Analogous statements hold for P ξ,− <sup>p</sup> according to line 9 of Algorithm 3.

Hence, the process noises are defined as Q L <sup>p</sup> = *q*¯ L I <sup>J</sup>×<sup>J</sup> and Q N <sup>p</sup> = *q*¯ <sup>N</sup> I <sup>J</sup>×<sup>J</sup> with small positive values *q*¯ L and *q*¯ <sup>N</sup> , respectively.

#### **Measurement update**

The measurement update from line 1 to line 4 of Algorithm 3 adapts *f* (*s*) based on (*s*p−1, <sup>y</sup>p−<sup>1</sup> ). The v-th dimension of yp−<sup>1</sup> refers to either *f* itself or a derivative of *f* . Therefore, the v-th row of the *V*p−<sup>1</sup> × *J* measurement matrix Cp−<sup>1</sup> reads

$$\mathcal{C}\_{p-1\_{v;1,\dots,I}} = \left(\mathbf{0}\_{1\times(\mu-(d+1))}, \frac{\partial^r}{\partial s^r} \mathbf{b}\_{\mu,d}(s\_{p-1}), \mathbf{0}\_{1\times(J-\mu)}\right),\tag{3.47}$$

Figure 3.5: Allocation of availabe data points and computed estimates xˆ to KF iterations in RBA versus MPF iterations in NRBA. Arrows indicate the needed information for computing the a priori estimates xˆ − and the a posteriori estimates xˆ + . By definition the MPF uses (s1, <sup>y</sup><sup>1</sup> ) for computing xˆ + 0 and sn for computing xˆ − n+1 . *Adopted from [78].*

whereby *<sup>s</sup>*p−<sup>1</sup> <sup>∈</sup> [κµ, κµ+1) and *<sup>r</sup>* <sup>∈</sup> <sup>N</sup>0. Algorithm 4 computes <sup>C</sup>p−<sup>1</sup> in line 3 using (3.47).

The value of the nonlinear measurement function *c* depends on the nonlinear particles xˆ <sup>N</sup>,−,<sup>p</sup> p−1 . Furthermore, *c* can depend on additional quantities that vary with the application and are not stated in Algorithm 3.

The diagonal *V*<sup>p</sup> × *V*<sup>p</sup> covariance matrix of measurement noise Rp−<sup>1</sup> enables a relative weighting of the dimensions of yp−<sup>1</sup> because the influence of the vth dimension of the measurement error e p <sup>p</sup> = yp−<sup>1</sup> − yˆ p on xˆ <sup>L</sup>,−,<sup>p</sup> p−1 and xˆ <sup>N</sup>,−,<sup>p</sup> p−1 decreases with a growing positive value Rp−1v;<sup>v</sup> .

#### **Time update with shift operation**

Based on a comparison between <sup>κ</sup> <sup>p</sup>−<sup>1</sup> and *<sup>s</sup>*p, NRBA decides if a shift operation of the B-spline function definition range is required to achieve that *s*<sup>p</sup> ∈ Dp.

The variable σ calculated from line 4 to line 17 of Algorithm 4 states the shift direction of <sup>D</sup>p−<sup>1</sup> and by how many positions components in <sup>κ</sup> <sup>p</sup>−1, <sup>x</sup><sup>ˆ</sup> <sup>L</sup>,−,<sup>p</sup> p−1 and xˆ <sup>N</sup>,−,<sup>p</sup> p−1 need to be moved for that purpose. σ > <sup>0</sup> indicates a right shift of <sup>D</sup>p−1, σ < <sup>0</sup> indicates a left shift, and σ <sup>=</sup> <sup>0</sup> means that no shift is conducted because *s*<sup>p</sup> ∈ Dp−1.

Algorithm 4 expects that, for σ > <sup>0</sup>, the <sup>|</sup>σ<sup>|</sup> additionally needed knots are the <sup>σ</sup> last entries of the knot vector <sup>κ</sup>¯ <sup>p</sup> <sup>=</sup> (κ¯ <sup>p</sup><sup>1</sup> , κ¯ p2 , . . ., <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> ) and that they are the <sup>−</sup><sup>σ</sup> first entries of <sup>κ</sup>¯ <sup>p</sup> if σ < <sup>0</sup>.

# **Case 1: Right shift of definition range (**σ <sup>≥</sup> <sup>0</sup>**)**

The new knot vector <sup>κ</sup> <sup>p</sup> is given by (3.18) and line 6 of Algorithm 3 updates xˆ <sup>L</sup>,+,<sup>p</sup> p−1 to xˆ <sup>L</sup>,−,<sup>p</sup> <sup>p</sup> using the state transition matrix

$$\mathcal{H}\_p^L = \mathcal{H}\_p \text{ from (3.19)}\tag{3.48}$$

and the input signal vector

$$
\boldsymbol{\mu}\_p^L = \boldsymbol{\mu}\_p \text{ from (3.21)}.\tag{3.49}
$$

Thereby all entries of xˆ <sup>L</sup>,+,<sup>p</sup> p−1 are moved to the left and the last σ entries of xˆ <sup>L</sup>,−,<sup>p</sup> <sup>p</sup> have an arbitrary initial value *x*¯:

$$\hat{\mathfrak{x}}\_{p}^{L,-,p} = \left(\hat{\mathfrak{x}}\_{p-1\_{\sigma+1}}^{L}, \hat{\mathfrak{x}}\_{p-1\_{\sigma+2}}^{L}, \dots, \hat{\mathfrak{x}}\_{p-1\_{J-\sigma}}^{L}, \vec{\mathfrak{x}}\mathbf{1}\_{l\times\sigma}\right)^{\top} \tag{3.50}$$

During a right shift of the definition range, *x*¯ is set to the last element of xˆ + p−2 , which is determined during the preceding call of Algorithm 3 in line 5. This is based on the assumption that xˆ + p−2 is a good initial value in the magnitude of the data.

Additionally, line 8 of Algorithm 3 updates P L,+ p−1 to P L,− <sup>p</sup> using (3.48) and

$$\mathbf{Q}\_p^L \in \mathbb{R}^{J \times J} \text{ with } \mathbf{Q}\_{p\_{\mathcal{G};h}}^L = \begin{cases} \bar{p}, & \text{if } h = \mathbf{g} \wedge \mathcal{Q} \\\\ \bar{q}^L, & \text{if } h = \mathbf{g} \wedge \neg \mathcal{Q} \\ 0, & \text{otherwise} \end{cases} \tag{3.51}$$

with *Q* given by (3.24). The update operation moves the elements in P L,+ p−1 to the top left and replaces the zeros on the last σ main diagonal elements of <sup>Q</sup> L p

with *<sup>p</sup>*¯ in order to get large values on the last σ main diagonal elements of <sup>P</sup> L,− p and a fast adaption of the initial estimates *x*¯ to the data points.

In line 7 and line 9, Algorithm 3 computes the quantities ξ<sup>ˆ</sup> p p and P ξ,− <sup>p</sup> that are needed for the PF time update. The calculations of the state transition matrix A<sup>N</sup> with

$$\mathcal{H}\_p^N = \mathcal{H}\_p \text{ from (3.19)}\tag{3.52}$$

and the input signal vector u <sup>N</sup> with

$$
\mathfrak{u}\_p^N = \mathfrak{u}\_p \text{ from (3.21)}\tag{3.53}
$$

are analogous to those for the linear quantities. Q N uses *q*¯ <sup>N</sup> instead of *q*¯ L :

$$\mathbf{Q}\_p^N \in \mathbb{R}^{J \times J} \text{ with } \mathbf{Q}\_{p\_{\mathcal{G}},h}^N = \begin{cases} \bar{p}, & \text{if } h = \mathbf{g} \wedge \mathcal{Q} \\ \bar{q}^N, & \text{if } h = \mathbf{g} \wedge \neg \mathcal{Q} \\ 0, & \text{otherwise.} \end{cases} \tag{3.54}$$

# **Case 2: Left shift of definition range (**σ < <sup>0</sup>**)**

The updated knot vector is given by (3.25), the input signal vector for linear states u L reads

$$
\boldsymbol{\mu}\_p^L = \boldsymbol{\mu}\_p \text{ from (3.26)}\tag{3.55}
$$

and the input signal vector for nonlinear states u <sup>N</sup> is given by

$$
\mathfrak{u}\_p^N = \mathfrak{u}\_p \text{ from (3.26)}.\tag{3.56}
$$

Additionally, *x*¯ is set to the first component of xˆ + p−2 .

Since A<sup>L</sup> p and A<sup>N</sup> p are identical in the chosen state-space model, computational effort can be saved when calculating the covariances and cross-covariances from line 8 to line 11 in Algorithm 3. *Adopted from [78]*.

### **3.4.4 Effectiveness of nonlinear recursive B-spline approximation**

 Numerical experiments conducted in [78] demonstrate the effectiveness of Algorithm 4 in comparison with solutions determined using the LM [115] algorithm. Therein effects of the number of simultaneously adaptable spline intervals and the particle count on the NRBA solution are also investigated. A MATLAB implementation of the experiments is provided in [76].

#### **General experimental setup**

The data set {(*s*p, <sup>y</sup><sup>p</sup> )}p=1,2,...,<sup>P</sup> is defined according to Section 3.2, whereby

$$\begin{aligned} \mathbf{s}\_p &= 0.25 + 0.5 \cdot (p - 1), \\ \mathbf{y}\_{p,1} &= \begin{cases} 40, \text{if } 80 \le \mathbf{s}\_p < 120 \\ 30, \text{otherwise} \end{cases} \\ \mathbf{y}\_{p,2} &= \mathbf{y}\_{p,3} = \mathbf{y}\_{p,4} = 0 \,\forall p \\ P &= 400. \end{aligned} \tag{3.57}$$

A B-spline function *<sup>f</sup>* (*s*) with knot vector κ <sup>=</sup> (−30, <sup>−</sup>20, . . ., <sup>230</sup>) and degree *<sup>d</sup>* <sup>=</sup> <sup>3</sup> approximates the data. Thereby, it is supposed that <sup>y</sup>p,<sup>1</sup> refers to *<sup>f</sup>* , <sup>y</sup>p,<sup>2</sup> to the first derivative of *<sup>f</sup>* , <sup>y</sup>p,<sup>3</sup> to the second derivative of *<sup>f</sup>* , and <sup>y</sup>p,<sup>4</sup> to the value of the nonlinear measurement function *<sup>c</sup>*, which is defined as a quadratic B-spline function with κ <sup>=</sup> (−5, <sup>0</sup>, . . ., <sup>70</sup>) and <sup>x</sup> <sup>=</sup> (0, <sup>0</sup>, <sup>0</sup>, <sup>0</sup>.25, <sup>1</sup>.5, <sup>5</sup>, <sup>5</sup>, <sup>0</sup>, <sup>0</sup>, <sup>6</sup>, <sup>8</sup>, <sup>8</sup>, <sup>8</sup>) > .

*c* depends on the value of the approximating function *f* (*s*) and is displayed in Figure 3.6. The input variable *f* (*s*) of *c* is restricted to the definition range [5, 60] of *<sup>c</sup>*.

The diagonal measurement covariance matrix R<sup>p</sup> ∈ R <sup>4</sup>×<sup>4</sup> with Rp1;1 = 1, Rp2;2 = 5·10−<sup>2</sup> , Rp3;3 = 5·10−<sup>3</sup> and <sup>R</sup>p4;4 <sup>=</sup> <sup>0</sup>.<sup>8</sup> or <sup>10</sup><sup>6</sup> , respectively, comprises

Figure 3.6: Nonlinear measurement function c f (s) depending on the value of the B-spline function f (s) approximating the data. *Adopted from [78].*

the reciprocal weights of <sup>y</sup>p,1, <sup>y</sup>p,2, <sup>y</sup>p,<sup>3</sup> and <sup>y</sup>p,4. The reciprocal weight values for the first three dimensions of y<sup>p</sup> avoid that *f* oscillates and cause that *f* smooths the jumps in the first dimension of the measurements. With Rp4;4 = <sup>0</sup>.8, the nonlinear target criterion *<sup>c</sup>* ( *<sup>f</sup>* (*s*)) <sup>=</sup> <sup>0</sup> is weighted strongly, whereas it is almost completely neglected with Rp4;4 = 10<sup>6</sup> .

Depending on the applied algorithm, solutions for the former weighting case are denoted by NRBA<sup>N</sup> or LM<sup>N</sup> indicating the nonlinear problem. Conversely, solutions for the latter case of a quasi-linear approximation problem are denoted by NRBA<sup>L</sup> or LM<sup>L</sup> .

Solutions for two different numbers of spline intervals *I* are analysed. For *I* = 1, <sup>κ</sup> is initialized with <sup>κ</sup><sup>0</sup> <sup>=</sup> (−30, <sup>20</sup>, . . ., <sup>40</sup>) which leads to an initial definition range [0, <sup>10</sup>) of *<sup>f</sup>* (*s*). For *<sup>I</sup>* <sup>=</sup> <sup>3</sup>, <sup>κ</sup> is initialized with <sup>κ</sup><sup>0</sup> <sup>=</sup> (−30, <sup>20</sup>, . . ., <sup>60</sup>) and the resulting definition range is [0, <sup>30</sup>). In both cases NRBA approximates the data by repeatedly shifting the function definition range to the right. Each time, an additional knot value <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> needs to be provided in the vector <sup>κ</sup>¯ <sup>p</sup>. For *<sup>I</sup>* <sup>=</sup> <sup>1</sup> these values are <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> <sup>=</sup> <sup>50</sup>, <sup>60</sup>, . . ., <sup>230</sup> and for *<sup>I</sup>* <sup>=</sup> <sup>3</sup> they read <sup>κ</sup>¯ <sup>p</sup><sup>K</sup> <sup>=</sup> <sup>70</sup>, <sup>80</sup>, . . ., <sup>230</sup>. In order to display the NRBA results for the whole data set, all values that are moved out of NRBA matrices and vectors are stored elsewhere.

The remaining NRBA parameters are *q*¯ <sup>L</sup> <sup>=</sup> <sup>0</sup>.005, *<sup>q</sup>*¯ <sup>N</sup> <sup>=</sup> <sup>0</sup>.<sup>25</sup> and *<sup>p</sup>*¯ <sup>=</sup> <sup>30</sup>. The LM algorithm uses *x*¯ Init = 30 as the initial value for each control point.

Due to the included PF, NRBA is a nondeterministic method. For each setting, 50 runs are performed and for each run the normalized root mean square error

(NRMSE) between the B-spline function determined by NRBA, *f* NRBA, and the B-spline function according to LM, *f* LM, is calculated as follows:

$$\begin{aligned} \text{INRMSE} &= \frac{1}{\max\_{p=1,...,P} \{f\_{\text{LM}}(s\_p)\} - \min\_{p=1,...,P} \{f\_{\text{LM}}(s\_p)\}} \\ &\cdot \sqrt{\frac{\sum\_{p=1}^P \left(f\_{\text{NRBA}}(s\_p) - f\_{\text{LM}}(s\_p)\right)^2}{P}} \end{aligned} \tag{3.58}$$

The terms NRBAmed and NRBAmax refer to the NRBA solution with the median or maximum NRMSE, respectively, in each set of 50 runs.

#### **Results**

Figure 3.7 shows for both the quasi-linear (L) and the nonlinear (N) problem the approximating functions NRBAmed and NRBAmax compared to the LM solutions. Black dots depict the first component <sup>y</sup>p,<sup>1</sup> of 40 of the 400 data points (*s*p, <sup>y</sup><sup>p</sup> ). Dashed vertical lines indicate knots, whereby the first and last knots are not shown.

Figure 3.7a shows NRBA approximations for *I* = 1 and *P* = 6561 = 9 4 . In this case the MPF state vector comprises four linear and four nonlinear components and the PF creates nine samples per nonlinear state dimension.

At *<sup>f</sup>* (*s*) <sup>=</sup> <sup>30</sup>, the deviation between the value of *<sup>c</sup>* and its target value <sup>y</sup>p,<sup>4</sup> <sup>=</sup> <sup>0</sup> has a local maximum (c.f. Figure 3.6). The nonlinear problem penalizes this deviation strongly; hence, NRBA<sup>N</sup> and LM<sup>N</sup> avoid *f* (*s*) = 30. In contrast, NRBA<sup>L</sup> and LM<sup>L</sup> approximate data with <sup>y</sup>p,<sup>1</sup> <sup>=</sup> <sup>30</sup> closely.

Data and knot vector are symmetrical to the straight line given by *s* = 100. Since the LM algorithm processes all data simultaneously in each iteration, the solutions LM<sup>L</sup> and LM<sup>N</sup> in Figure 3.7a reflect this symmetry.

In contrast, NRBA processes the data from left to right and can only adapt some control points at a time. For *I* = 1, these are the four control points that

Figure 3.7: B-spline functions f with NRBA for interval counts I and particle counts P and with median (med) and maximum (max) NRMSE compared to the LM solution for quasilinear (L) and nonlinear (N) problems. Only <sup>40</sup> data points (sp, <sup>y</sup>p,1) and a subset of knots κ is shown. *Adopted from [78].*

influence the B-spline function in the spline interval in which the current data point lies and for *I* = 3 additionally the two control points that affect the two spline intervals to the left.

NRBA<sup>L</sup> and NRBA<sup>N</sup> are both asymmetrical and mostly delayed with respect to LM<sup>L</sup> and LMN. For NRBA<sup>N</sup> , the asymmetry is less distinct because the PF removes states that translate to a large delay more quickly from the particle set because they create a larger error. Additionally, the range of values in NRBA<sup>N</sup> is smaller than in NRBA<sup>L</sup> so that a present lag is less obvious.

For the same weighting NRBAmed and NRBAmax differ only slightly, which suggests that, for the given setup, *P* = 6561 suffices for a convergence of NRBA solutions.

With RBA for linear weighted least squares approximation similar numerical experiments but without any nonlinear approximation criterion were performed in Subsection 3.3.4 and [80]. For *I* = 1, a strong asymmetry and delay are observed with RBA, analogous to NRBA<sup>L</sup> in Figure 3.7a. The delay decreased as *I* was increased because the filter was able to update more control point estimates with hindsight based on P L,+ .

By increasing *I* to three with NRBA, the dimension of the state space also increases to six linear and six nonlinear components. The PF in NRBA samples the state space less densely, unless the particle count is increased exponentially with *I*.

Figure 3.7b displays the results for the quasi-linear approximation problem for the case of keeping the sampling density per nonlinear state space dimension constant by choosing *P* = 625 = 5 4 for *I* = 1 and *P* = 15625 = 5 6 for *I* = 3. Overall the NRBA solution is more symmetrical with *I* = 3 but a comparison of NRBAmed for *I* = 1 and *I* = 3 indicates that the delay for *s* ≥ 120 is not reduced. Instead, the ability to adapt more control point estimates simultaneously sometimes leads to undesirable results, e.g. the too low course between *s* = 40 and *s* = 60 as well as the overcompensation of the delay between *s* = 60 and *s* = 75.

For *I* = 1, NRBAmax differs more from NRBAmed and shows larger oscillation amplitudes than for *I* = 3. This suggests that *P* = 625 is not sufficient for a convergence of NRBA for *I* = 1. However, even with 625 particles for *I* = 1, the required increase to *P* = 15625 for *I* = 3 is already quite strong. Keeping the sampling density constant quickly becomes infeasible, especially if computation time constraints are present [70].

All other factors held constant Figure 3.7c shows the results for the nonlinear approximation problem, which support the previously drawn conclusions. Additionally, the conflicting target criteria in the nonlinear approximation problem cause a larger stabilization period at *s* ≤ 20.

Figure 3.7d depicts the effect of increasing *I* from one to three while maintaining the particle count of Figure 3.7a. For *I* = 3 NRBAmax differ much more from the corresponding NRBAmed. This indicates that more particles are needed for convergence for *I* = 3. Furthermore, for *I* = 3 these differences are much larger for NRBA<sup>N</sup> than for NRBA<sup>L</sup> .

A comparison between corresponding NRBAmed solutions in both figures shows only a small approximation improvement from increasing *I* for the chosen setup. In Figure 3.7d NRBA<sup>N</sup> temporarily decreases below *f* (*s*) = 30, the position of the maximum of *c* (c.f. Figure 3.6). This is a case in which NRBA chooses a locally but not globally good solution. More detailed investigations on the effects of particle count *P* on convergence can be found in the cited prepublication. *Adopted from [78]*.

## **3.5 Scientific contribution**

This chapter presented two novel algorithms for the weighted least squares (WLS) approximation of a set of data points with a B-spline function. These are the algorithm recursive B-spline approximation (RBA) for the case of a linear WLS problem and the algorithm nonlinear recursive B-spline approximation (NRBA) for the case of a nonlinear WLS problem.

Figure 3.8: Classification of unconstrained weighted least squares (WLS) B-spline approximation problems and suitable algorithms.

Table 3.1: Comparison of B-spline approximation methods. <sup>∗</sup> : Only applies to WLS. ∗∗: Also applies to LM if the sliding window method in [38] is used with a window size very small compared to P. *Adopted from [80].*


Figure 3.8 subdivides the unconstrained WLS B-spline approximation problem and illustrates the problem types to which RBA, NRBA and the well-known algorithms WLS, RLS, KF and LM can be applied.

Table 3.1 compares features of the mentioned approximation methods. With LM the computational effort in each iteration depends on the number of data points *P*. However, the number of iterations performed depends on the specified tolerance and cannot be deduced solely from *P*. Therefore the time complexity statement in Table 3.1 does not apply to LM in the general case. However, the sliding window LM approach in [38] has time compexity O(*P*) if the window size is very small compared to *P*.

The design of RBA, NRBA in this chapter as well as their applications in the following chapters are limited to the B-spline representation and functions. However, the limitation to functions and B-splines in this work did not lead to a loss of generality and the approach of RBA and NRBA can be generalized to curves and surfaces of all spline representations that offer local control, so that the benefits of RBA and NRBA stated in the following can be directly transferred to them as well.

This is because according to Section 2.3, curves and surfaces are direct extensions of functions. Moreover, spline representations with local control only differ in the geometry matrix M and therefore in the blending functions, but they all share the same structure of (2.17) for curves and of (2.19) for surfaces. Their local control results from the bounded interval, in which a blending function is nonzero. As a result, other local spline representations suffer from the same identified research gap.

#### **Recursive B-spline approximation**

 The RBA algorithm prepublished in [80] solves a linear WLS approximation problem iteratively using a KF which estimates the control points of the Bspline function sequentially. Therefore the total computational effort increases linearly with the number of approximated data points.

The main contribution is to use the time update of the KF for a shift of estimated B-spline control points in the KF state vector in combination with a shift in the B-spline knot vector. The shift operation enables to shift the definition range such that it is always possible to take into account the latest data point for the approximation.

Thereby RBA overcomes the limitation of other recursive algorithms based on the KF that can only approximate data points within the initially chosen and fixed approximation interval.

RBA is especially advantageous in online applications in which the magnitude of the data points is not exactly known or changes over time because then data points outside the initially chosen bounded B-spline function definition range can occur.

RBA is also beneficial when a tradeoff between low computational effort and high approximation quality is needed because the shift operation of RBA allows to reduce the size of the KF state vector in both online and offline applications. A smaller state vector causes less computational effort.

Numerical experiments in [80] and Subsection 3.3.4 show that the RBA result converges to the WLS solution as the size of the state vector is increased. Additionally the experimental results reveal that few simultaneously adaptable spline intervals suffice for good approximation results.

The number of required shift operations can increase when the size of the state vector is reduced to lower the computational effort. The experiments indicate that shift operations influence the control point values of the resulting approximation function only in a magnitude that is close to numerical accuracy. Each shift operation comes at the expense that a part of the approximation result is forgotten in order to keep the sizes of matrices and vectors constant. A growing approximation interval can be realized by storing matrix and vector elements separately from RBA before they are overwritten. *Adopted from [80]*.

#### **Nonlinear recursive B-spline approximation**

 The NRBA algorithm prepublished in [78] is a generalization of RBA for NWLS problems which result from target criteria that depend on the control points nonlinearly.

NRBA determines a B-spline function such that it approximates an unbounded number of data points with respect to both linear and nonlinear target criteria. The approach uses a MPF for solving the approximation problem iteratively.

In the MPF, a PF takes into account target criteria that do not relate to the control points in a linear fashion whereas a KF solves any linear subproblem optimally for each particle [70]. As the values of the B-spline function and its derivatives depend linearly on the control point values, linear target criteria will occur in most approximation applications.

The MPF can take into account the exactly known values of the B-spline basis functions and does not need to estimate them like most other nonlinear filters do. Taking advantage of the linear substructure of the problem allows to reduce computational effort and achieve better results compared to purely nonlinear filters like a PF [155].

The features and benefits of RBA that result from the shift operation also apply to NRBA. Numerical experiments in [78] and Subsection 3.4.4 investigated the effectiveness of the approach in comparison to the LM algorithm and illustrated the effects of selected NRBA parameter values on the approximation result. Provided that the parameters are chosen appropriately, the solution of the proposed method is close to the LM solution apart from a slight filter-typical delay.

NRBA use cases are NWLS problems in which a linearization of nonlinear criteria is not desired or promising, for example because of distinct nonlinearities.

For linear WLS problems RBA should be used instead of NRBA. RBA is based on the KF, which computes an optimal solution [205]. For linear problems NRBA can at best reach the same approximation quality provided that the particle count is large enough, which requires more computational effort.

Furthermore, with NRBA the approximation depends more strongly on the parameterization of the underlying filter algorithm than with RBA.

Increasing the number of control points that NRBA can adapt simultaneously is not as unambiguously beneficial for the approximation result as with RBA. Moreover, with NRBA the increase of control point count usually needs to be combined with an exponential increase of the particle count in the PF for an

(a) Trajectory optimization. Planned trajectory time *t*.

(b) Signal representation. Real time *t* measured from present point in time, interval *dt* between consecutive measurements.

Figure 3.9: Exemplary use cases for different operating modes of RBA and NRBA. Optimization range (OR).

improvement of the approximation. This is known as curse of dimensionality and a general problem of sampling-based nonlinear filters. *Adopted from [78]*.

#### **Use cases in terms of operating mode**

Use cases of RBA and NRBA can be classified according to whether the knot vector changes only with a shift operation or with each iteration:

• Knots change only with each shift operation: This mode assumes that the independent variable will not take values large enough to create an arithmetic overflow during operation. Therefore the knot vector changes only with a shift operation. Figure 3.9a illustrates this mode at the example of trajectory optimization, whereby the independent variable *t* refers to the planned trajectory time. From the top plot to the bottom plot the computation time progresses and the number of processed data points increases. In the example, *t* grows with the data, but a limited planning horizon, that does not lead to arithmetic overflows, can be assumed. Ideally, *t* increases much faster than real time so that there is no need to evaluate the trajectory in the optimization range (OR), in which it can change. Chapter 5 will investigate this use case in more detail. The examples in Subsection 3.3.4 and Subsection 3.4.4 also belong to this operating mode.

• Knots change with each iteration: This operating mode is required in case of an unbounded independent variable, that eventually will take a value that causes an arithmetic overflow. Figure 3.9b illustrates this mode at the example of an analytic representation of a signal or its derivative or its integral over the recent past up to the present point in time. From the top plot to the bottom plot a new measurement is received with each plot. If the independent variable *t* were assigned to the observation number, eventually an overflow in the knot vector would be encountered. Let *dt* denote the time interval between the two latest measurements. For simplicity, let *dt* be constant. An overflow can be avoided by subtracting *dt* from the knot vector after each iteration. Thereby the latest measurement is always assigned to *t* = 0 and the knots are continuously shifted to the left in Figure 3.9b as their values keep decreasing until they are removed from the knot vector and the OR of RBA or NRBA. The signal representation application requires to evaluate the function within the OR unless a time delay of the magnitude of the OR is acceptable and a some knots and control points can be saved when they leave the OR. If so, the function can be evaluated outside of the OR and further into the past.

#### **Differentation from previous works**

In the previous project *e-generation* [22, pp. 29-35] an iterative data approximation by a functional representation was developed as well and was patented by F. Bleimund and S. Rhode [21]. This previous approach has in common with RBA that it also solves the linear WLS optimization problem (3.13) iteratively, whereby the target criteria refer to the value of the functional representation and its first two derivatives. Moreover, the optimization problem is solved iteratively using a KF and a shift matrix.

However, in the previous project, a polynomial as in (2.8) is adapted to the data. This polynomial exhibits the features discussed in Section 2.3 along with its flaws compared to a spline representation, such as coupling between degrees of freedom and polynomial degree, risk of oscillations from the Runge's phenomenon, global effect of coefficient value changes and geometrically difficult to interpret coefficients.

Due to the global control of the polynomial, a shift operation and therefore the KF would not be needed if the approximation were performed analogously to the case depicted by Figure 3.9a, in which the value of the independent variable can take large values. For this case, RLS would suffice.

However, the approach assigns the latest oberserved data point always to the same value of the independent variable, analogous to Figure 3.9b. This is achieved by defining the shift operation via an upper triangular matrix such that a coefficient vector is calculated for a polynomial that is shifted by *dt* to the left with respect to the current polynomial during the time update of the KF. This shift operation for the polynomial corresponds to the subtraction of *dt* from the spline knot vector as described above for the second operating mode.

In contrast, the shifting operation stated in the previous sections of this work is needed in general for both of the above described operating modes. It modifies both the knot vector and the control point vector and occurs depending on the knot vector and the independent variable of the latest data point.

The algorithms presented in this work are superior to the one developed in *e-generation* in that they allow to benefit from the advantages of spline representations with local control during an iterative data approximation. The NWLS problem adressed by NRBA was not considered in the previous project.

# **4 Models of research vehicle and reference route**

This chapter describes models of the research vehicle and the reference route. The content of this chapter and the previous one will then be combined for trajectory optimization in Chapter 5 and ALC in Chapter 6.

The research vehicle is an all-wheel-driven BEV based on the Porsche Boxster (type 981) which was developed during the research project *e-generation*. Further information about the vehicle is provided in the remainder of this chapter as well as in [14, 16, 207].

Detailed nonlinear models allow a representation of the dynamic vehicle behavior that is close to reality. Such models take into account the kinematics of the vehicle and of its subsystems like the wheel suspension. They also comprise models of tire forces. However, a model should also focus on the aspects that are important for the actual application and simplify where possible. Depending on the desired level of detail and the application, different vehicle models are known in literature [157, pp. 5-6].

The purpose of the vehicle models in this work is to investigate the energy consumption of the research vehicle under ALC on a reference route. According to [187, p. 28] with a normally experienced driver the absolute value of the lateral acceleration is below <sup>3</sup>.5 m/<sup>s</sup> 2 and the longitudinal acceleration ranges roughly from <sup>−</sup>2.4 m/<sup>s</sup> 2 to <sup>1</sup>.8 m/<sup>s</sup> 2 . For comfort and acceptance reasons, an ALC should not exceed these limits most of the time, therefore models for low dynamics suffice. Simplifying assumptions of this work for longitudinal dynamics include:

1801 Figure 4.1: Lateral view of research vehicle at slope of angle α with acting climbing force <sup>F</sup>cl, rolling resistance Froll, cornering resistance Fcor, air resistance Fair and inertial force Finert. Vehicle drawing from [46].


1.0m 0.5m5cm

Scale: 1:25


For describing the lateral dynamics the linear single track model is used.

Section 4.1 states the coordinate system. Section 4.2 describes relevant driving resistances and Section 4.3 the power train of the research vehicle. Section 4.4 explains how the resulting energy consumption for a given route can be derived and reasons the approach for optimization of energy consumption that the ALC takes. Section 4.5 describes vehicle models for ALC tests in a simulation environment and for investigations of the resulting energy consumption of the research vehicle. The reference route is mentioned in Section 4.6 and Section 4.7 derives two simplified adaptive vehicle models for use within the ALC.

1282

### **4.1 Coordinate system**

Several coordinate systems are used to describe vehicle position and orientation [157, pp. 17-31]. This work uses a body-fixed right-hand vehicle coordinate system with origin in the vehicle COG and axes *x*, y and *z*. The axes are perpendicular to each other and shown in Figure 4.1 and Figure 4.2. The *x* axis is the vehicle longitudinal axis and points towards the vehicle front, whereas the y axis is the vehicle lateral axis that points towards the left vehicle side. Since rolling and pitching movements of the vehicle with respect to the road surface are neglected, the plane that *x* and y axes span is parallel to the road surface. The *z* axis is the vertical axis that points upwards and is perpendicular to the road surface.

### **4.2 Driving resistances**

This section states the main forces acting on a vehicle and the force equilibrium that they form at the wheels. Figure 4.1 and Figure 4.2 illustrate these forces.

### **4.2.1 Climbing force**

 The climbing force *F*cl results from the gravitational force acting on the COG and is given by

$$F\_{\rm cl} = m\_{\rm v\rm hcl} \cdot g \cdot \sin(\alpha). \tag{4.1}$$

*<sup>F</sup>*cl depends on the vehicle mass *<sup>m</sup>*vhcl and road slope angle <sup>α</sup> which is measured between the road surface and the plane that is perpendicular to the direction of gravitational force *m*vhcl · g with gravitational constant g. *Adopted from [79]*. The road slope angle α can be computed from the road slope γ stated in percent using

$$
\alpha = \arctan(\gamma/100) \text{ [119, p. 95].} \tag{4.2}
$$

### **4.2.2 Wheel resistance**

The wheel resistance consists of several components, of which only rolling resistance and cornering resistance will be considered. Resistance components neglected in this work include the toe-in resistance and flood resistance. The toein resistance is caused by not perfectly parallel wheels and the flood resistance occurs on wet roads when the tire evacuates water [119, pp. 11-19]. Moreover, the longitudinal tire slip resistance [165] and the ventilation resistance of the rotating wheel [182] are not considered.

Bearing friction as well as residual braking torque from the hydraulic brakes can also be assigned to wheel resistance but will be considered as resistances of the power train in this work.

#### **Rolling resistance**

 Rolling resistance results from damping forces of the deformed tire rubber. When driving straight on a dry road, the wheel resistance almost entirely comes from the rolling resistance *F*roll [119, p. 11] with

$$F\_{\rm roll} = f\_{\rm r} \cdot m\_{\rm v\rm hcl} \cdot g \cdot \cos(\alpha). \tag{4.3}$$

*Adopted from [79]*. The normal force *<sup>m</sup>*vhcl · <sup>g</sup> · cos(α) equals the sum of vertical front axle wheel force *F*Whl,z,FA and vertical rear axle wheel force *F*Whl,z,RA. In the static case the distribution of the vertical forces between front axle (FA) and rear axle (RA) can be determined from the wheel base (*l*), the distance between FA and COG (*l*FA) and the distance between RA and COG (*l*RA). The road slope influences the static load distribution between the axles as well but since the distance between COG of the research vehicle and road surface is small, this effect of road slope is neglected.

The rolling resistance coefficient *f*<sup>r</sup> depends on the vertical wheel force, tire pressure and slightly increases with velocity [24, pp. 50, 52-53]. For most passenger vehicles *f*<sup>r</sup> can be assumed between 0.007 and 0.014 [10].

#### **Cornering resistance**

In curves the centrifugal force *F*centr with

$$F\_{\text{centr}} = m\_{\text{vhcl}} \cdot \dot{\nu}\_{\text{Vhcl,y}} \tag{4.4}$$

acts on the COG. The lateral acceleration v˙Vhcl,y is given by

$$
\dot{\nu}\_{\text{Vhcl,y}} = \nu\_{\text{Vhcl}}^2 \cdot \kappa \tag{4.5}
$$

for small side slip angles and constant velocity [157, p. 228]. The road curvature κ is the reciprocal of the curve radius.

The single track vehicle model represents the fundamental driving dynamics for lateral acceleration smaller than <sup>v</sup>˙Vhcl,y <sup>=</sup> 4 m/<sup>s</sup> <sup>2</sup> on dry roads. The model is very idealized and uses the following simplifications:


4374 1801 2475 Figure 4.2: Top view of research vehicle with acting air forces. Environmental wind with velocity <sup>v</sup>Wind and angle <sup>τ</sup>wind as well as vehicle velocity <sup>v</sup>Vhcl result in relative air flow with velocity <sup>v</sup>rel and angle <sup>τ</sup>rel. This causes longitudinal air resistance <sup>F</sup>air,x and lateral air resistance Fair,y acting on the pressure point (PP). COG denotes the center of gravity. Vehicle drawing from [46].

In order to stay on the road, the tires need to provide lateral forces that counteract the centrifugal force. These lateral forces are distributed among the axles depending on the position of the COG and created using tire slip angles.

Porsche Boxster 981 (2013)

These slip angles cause the cornering resistance *F*cor acting in longitudinal direction. Under the assumption of small road curvature, small steering angle, small slip angles and identical tires, *F*cor according to the single track model reads 

$$F\_{\rm cor} = \frac{\kappa^2 \cdot \nu\_{\rm V\_{\rm hcl}}^4}{2 \cdot c} \cdot \left[ \left( \frac{I\_{\rm FA}}{l} \cdot m\_{\rm vhcl} \right)^2 + \left( \frac{I\_{\rm RA}}{l} \cdot m\_{\rm vhcl} \right)^2 \right] \tag{4.6}$$

whereby *c* denotes the tire cornering stiffness [64, p. 147].

#### **4.2.3 Air resistance**

The air resistance *F*air acts on the pressure point (PP) of the vehicle and results from the turbulences of air stream and friction of the air flow. The relative air velocity <sup>v</sup>rel and relative air flow angle <sup>τ</sup>rel both depend on the vehicle velocity

1282

vVhcl, the environmental wind velocity vWind and the environmental wind angle <sup>τ</sup>wind as Figure 4.2 illustrates:

$$\begin{aligned} \nu\_{\rm rel} &= \sqrt{\nu\_{\rm Vhcl}^2 + \nu\_{\rm Wind}^2 + 2 \cdot \nu\_{\rm Vhcl} \cdot \nu\_{\rm Wind} \cdot \cos(\tau\_{\rm wind})} \\ \tau\_{\rm rel} &= \arcsin\left(\frac{\nu\_{\rm Wind}}{\nu\_{\rm rel}}\right) \cdot \sin(\tau\_{\rm wind}) \end{aligned} \tag{4.7}$$

The angles are measured anticlockwise starting from negative direction of the *x* axis. The longitudinal air resistance *F*air,x is given by

$$F\_{\rm air,x} = c\_{\rm x} \left(\tau\_{\rm rel}\right) A \frac{\rho}{2} \nu\_{\rm rel}^2. \tag{4.8}$$

*c*<sup>x</sup> is the dimensionless longitudinal drag coefficient that describes the shape of the vehicle and depends on the relative air flow angle τrel. *<sup>A</sup>* denotes the effective vehicle cross-sectional area perpendicular to the vehicle longitudinal axis and ρ is the air density. *Adopted from [79]*. Analogous to the centrifugal force, the lateral air resistance *F*air,y needs to be balanced by lateral tire forces. The required slip angles in turn cause a driving resistance component in longitudinal direction.

Neglecting environmental wind leads to <sup>v</sup>rel <sup>=</sup> <sup>v</sup>Vhcl, <sup>τ</sup>rel <sup>=</sup> <sup>0</sup> and *<sup>F</sup>*air,y <sup>=</sup> <sup>0</sup>. *<sup>c</sup>*x(τrel <sup>=</sup> <sup>0</sup>) is also denoted *<sup>c</sup>*<sup>w</sup> [157, pp. 212-214]. For passenger vehicles *<sup>c</sup>*<sup>w</sup> usually lies between <sup>0</sup>.<sup>20</sup> and <sup>0</sup>.<sup>40</sup> and *<sup>A</sup>* ranges from <sup>1</sup>.5 m<sup>2</sup> to <sup>2</sup>.5 m<sup>2</sup> . With increasing vehicle velocity *F*air,x becomes the largest driving resistance component [24, pp. 50-51].

#### **4.2.4 Inertial force**

Inertial forces arise when vehicle velocity changes. Analogously variations of the angular velocity of power train components cause inertial torque. Each axle of the research vehicle is driven independently as Figure 4.3 illustrates. Due to the fixed gear ratio of each gear box and the assumption of negligible longitudinal tire slip, angular velocities are proportional to the vehicle velocity. Computing the equivalent mass of the power train allows to summarize both inertial force and inertial torque in the inertial force *F*inert with respect to the vehicle longitudinal acceleration v˙Vhcl,x [62, pp. 16-17]:

$$F\_{\rm inert} = (m\_{\rm v\rm hcl} + m\_{\rm eq, FA} + m\_{\rm eq, RA}) \cdot \dot{\nu}\_{\rm V\rm hcl, x} \tag{4.9}$$

*<sup>m</sup>*eq,F A is the equivalent mass of the power train components that drive the front axle FA and *<sup>m</sup>*eq,RA denotes the equivalent mass of components that drive the rear axle RA:

$$\begin{aligned} m\_{\text{eq,FA}} &= \frac{J\_{\text{EM,FA}} \cdot i\_{\text{G}}^2 + J\_{\text{GB}} + 2 \cdot J\_{\text{Wheel,FA}} + 2 \cdot J\_{\text{Brak},\text{FA}}}{r\_{\text{dyn,FA}}^2} \\ m\_{\text{eq,RA}} &= \frac{J\_{\text{EM,RA}} \cdot i\_{\text{G}}^2 + J\_{\text{GB}} + 2 \cdot J\_{\text{Wheel,RA}} + 2 \cdot J\_{\text{Brak},\text{RA}}}{r\_{\text{dyn,RA}}^2} \end{aligned} \tag{4.10}$$

*J*EM is the mass moment of inertia of the corresponding electric motor (EM) that is connected via a gear box (GB) with integrated differential to the wheels. Both gear boxes have the same gear ratio *<sup>i</sup>*<sup>G</sup> <sup>=</sup> <sup>9</sup>.<sup>59</sup> [14]. *<sup>J</sup>*GB is the mass moment of inertia of a GB, *J*Wheel the mass moment of inertia of a wheel, *J*Brake the mass moment of inertia of a brake and *r*dyn the dynamic wheel radius.

#### **4.2.5 Force equilibrium**

 The sum of the driving resistances from (4.1), (4.3), (4.6), (4.8) and (4.9) equals the traction force *F*trac between all tires and the road surface:

$$F\_{\rm trac} = F\_{\rm cl} + F\_{\rm roll} + F\_{\rm air} + F\_{\rm cor} + F\_{\rm inert} \tag{4.11}$$

The mechanical traction power *P*trac,mech corresponding to *F*trac reads

$$P\_{\text{trace,mech}} = F\_{\text{trace}} \cdot \nu\_{\text{Vhcl}}.\tag{4.12}$$

Figure 4.3: Model of power train of research vehicle.

Providing *F*trac requires longitudinal tire slip. Under the assumption of negligible slip losses, *P*trac,mech equals the wheel power *P*wheel provided by the power train [157, pp. 152-153]. *Adopted from [79]*.

## **4.3 Power train**

Figure 4.3 depicts the power train of the research vehicle. In various components of the power train power losses occur. These power losses are measured during component and vehicle tests on test benches and are stored in look-up tables, which have been provided for the research vehicle. Depending on the kind of input power, components of the power train can be divided into mechanical components and electrical components.

#### **4.3.1 Mechanical components**

The wheel power *P*wheel provided by the power train reads

$$P\_{\rm wheel} = T\_{\rm FA} \cdot \omega\_{\rm Whl, FA} + T\_{\rm RA} \cdot \omega\_{\rm Whl,RA} \tag{4.13}$$

with front axle torque *T*FA, rear axle torque *T*RA, front wheel angular velocity <sup>ω</sup>Whl,FA and rear wheel angular velocity <sup>ω</sup>Whl,RA.

The torque distribution between *T*FA and *T*RA is computed by the motor electronic control unit (ECU) as a function of *F*trac. At each axle a gear box (GB) with open differential distributes the torque equally among the left and right wheel.

When driving straight, <sup>ω</sup>Whl,FA and <sup>ω</sup>Whl,RA are given by

$$
\omega\_{\text{Whl,FA}} = \frac{\nu\_{\text{Vhcl}}}{r\_{\text{dyn,FA}}}, \quad \omega\_{\text{Whl,RA}} = \frac{\nu\_{\text{Vhcl}}}{r\_{\text{dyn,RA}}} \tag{4.14}
$$

with dynamic front wheel radius *<sup>r</sup>*dyn,FA and dynamic rear wheel radius *<sup>r</sup>*dyn,RA. Between wheel and gear box undesired friction occurs in the wheel bearing, which results in braking torque. There is also residual brake torque caused by permanent residual friction between brake pad and brake disc. The model takes into account the front axle brake torque *T*Brk,FA and the rear axle brake torque *T*Brk,RA. The values are derived by interpolating linearly with respect to the wheel angular velocities in look-up tables.

At each axle a 1-speed gear box with gear ratio *i*<sup>G</sup> increases the motor angular velocity and reduces the motor torque. Front axle motor angular velocity <sup>ω</sup>EM,FA and rear axle motor angular velocity <sup>ω</sup>EM,RA are given by

$$
\omega\_{\rm EM,FA} = i\_{\rm G} \cdot \omega\_{\rm Whl,FA}, \quad \omega\_{\rm EM,RA} = i\_{\rm G} \cdot \omega\_{\rm Whl,RA}.\tag{4.15}
$$

Front axle motor torque *T*EM,FA and rear axle motor torque *T*EM,RA read

$$\begin{aligned} T\_{\rm EM,FA} &= \frac{T\_{\rm FA}}{i\_{\rm G}} + T\_{\rm Brk,GB,FA} \left( T\_{\rm EM,FA}, \omega\_{\rm EM,FA} \right), \\ T\_{\rm EM,RA} &= \frac{T\_{\rm RA}}{i\_{\rm G}} + T\_{\rm Brk,GB,RA} \left( T\_{\rm EM,RA}, \omega\_{\rm EM,RA} \right), \end{aligned} \tag{4.16}$$

whereby *T*Brk,GB,FA and *T*Brk,GB,RA denote the braking torques occurring in the gear boxes during the not lossless power conversion. The absolute values of these quantities increase with the absolute values of motor torque and angular velocity and are determined using two-dimensional linear interpolation in lookup tables.

#### **4.3.2 Electrical components**

A permanently excited synchronous motor (PSM) with 120 kW drives the front axle and an asynchronous motor (ASM) with 144 kW drives the rear axle. The PSM has a higher efficiency whereas the ASM has lower drag losses. Therefore, mainly the PSM propels the vehicle and is supported by the ASM only in case of high power demands [14]. The motors are connected to the high voltage (HV) battery via power electronics (PE). The losses of both motors including the losses in the power electronics are modelled by two-dimensional linear interpolation with respect to motor torque *T*EM and motor angular velocity <sup>ω</sup>EM in look-up tables. Ohmic resistance and magnetic resistance as well as switching losses contribute to the losses of EM and PE. *P*trac,elec is the electrical traction power that results from the mechanical traction power *P*trac,mech and includes the power losses between power electronics and wheels.

While braking, the motors act as generators and convert mechanical traction power *P*trac,mech into electrical traction power *P*trac,elec that is recuperated into the HV battery. For driving stability reasons, the majority of the recuperation power comes from the front wheels, which are connected to the more efficient PSM. When the braking power demand exceeds the recuperation capabilites, hydraulic wheel brakes are activated in order to fulfill the demand. Hydraulic

wheel brakes convert mechanical traction power into thermal power that cannot be reused for vehicle propulsion anymore. Additional hydraulic braking power is usually needed when braking at high velocity or when high deceleration is requested. Furthermore, during a standstill and at very low speeds hydraulic brakes are solely used.

Due to the powerful drive train, the vehicle has a high recuperation power limit. Furthermore, simulations of the ALC for determining the energy consumption will not include standstills, high dynamic driving maneuvers and driving at high speeds. Therefore this work assumes that the vehicle can brake solely using recuperation, also denoted perfect recuperation by [62, p. 16], and does not consider hydraulic braking power in the vehicle model.

Apart from the power electronics, the motors and the HV battery, a positive temperature coefficient (PTC) battery heating, an air conditioning (AC) compressor and a DC/DC converter for the 12 V circuit belong to the HV system. The on-board network (OBN) is fed via the 12 V battery or directly from the DC/DC converter. The on-board network (OBN) also powers a thermoelectric heat pump for heating the passenger cabin. Arrows in Figure 4.3 indicate possible power flow directions between electrical components.

As a result the PTC power consumption *P*PTC, the AC power consumption *P*AC and the DC/DC converter power consumption *P*DC/DC also contribute to the HV battery power demand *P*Batt,dem:

$$P\_{\rm Batt,dem} = P\_{\rm trac,elec} + P\_{\rm PTC} + P\_{\rm AC} + P\_{\rm DCDC} \tag{4.17}$$

In case of recuperation *P*Batt,dem can become negative indicating that power is transferred to the battery.

The HV battery consists of four cell strings arranged in parallel. Each cell string is composed of 100 lithium nickel-manganese-cobalt oxide (LiNMC) cells connected in series [207]. The cells are liquid cooled and can be heated using PTC elements. The battery can provide an electrical power of 300 kW at a nominal voltage of 370 V [14]. Its nominal capacity of 38.3 kWh enables a

vehicle range of more than 200 km [16]. The grid charging power *P*grid is up to 22 kW with AC via the battery charger inside the vehicle and up to 100 kW with DC [14].

According to [71] the terminal voltage *U*TV of a LiNMC battery can be described using the following model stated in [134]:

$$U\_{\rm TV} = U\_{\rm OCV}(SOC) - R \cdot I$$

$$R = \begin{cases} R^-, & I \ge 0 \\ R^+, & I < 0 \end{cases} \tag{4.18}$$

Hereby *U*OCV is the open-circuit voltage as a function of state of charge (SOC). The internal ohmic resistance *R* can be separated into charge resistance *R* + and discharge resistance *R* − . The battery current *I* is assumed positive for discharge and negative for charge. *U*OCV, *R* + and *R* − are determined by cell testing [134] and have been provided for the HV battery of the research vehicle in look-up tables.

Figure 4.4 shows the equivalent circuit from [134] and the approximate opencircuit voltage derived from [71]. A comprehensive literature review of battery models and their application to LiNMC batteries is given in [71].

According to (4.18) the internal ohmic resistance of the battery causes a HV battery power loss *P*Batt,Loss, during a HV battery power demand *P*Batt,dem:

$$P\_{\text{Bant,dem}} = U\_{\text{TV}} \cdot I = \begin{cases} \underbrace{U\_{\text{OCV}} \cdot I}\_{=: P\_{\text{Bant}} > 0} - \underbrace{R^{-} \cdot I^{2}}\_{=: P\_{\text{Bant,Loa}} > 0}, & P\_{\text{Bant,dem}} > 0 \\ \underbrace{U\_{\text{OCV}} \cdot I}\_{=: P\_{\text{Bant}} < 0} - \underbrace{R^{+} \cdot I^{2}}\_{=: P\_{\text{Bant,Loa}} > 0}, & P\_{\text{Bant,dem}} < 0 \end{cases} \tag{4.19}$$

This means that if the HV battery power *P*Batt is positive, i.e. the battery is discharged, the battery must provide *P*Batt,Loss in addition to *P*Batt,dem in order to compensate for its ohmic losses. As a result, the SOC will decrease faster.

Figure 4.4: High voltage battery characteristics. Top: Equivalent circuit according to the "simple model" in [134] with state of charge (SOC), open-circuit voltage UOCV, charge resistance R <sup>+</sup>, discharge resistance R <sup>−</sup>, battery current I and terminal voltage UTV. The diodes are assumed ideal. Bottom: Exemplary UTV of the LiNMC battery in the research vehicle depending on SOC and voltage across the internal ohmic resistance. UOCV was derived from [105].

Conversely, if *P*Batt is negative because of recuperation, the SOC will increase slower because the absolute value of the charging power is reduced by *P*Batt,Loss.

## **4.4 Energy consumption and optimization approach**

 Coming from a temporal representation, the HV battery energy *E*Batt needed for a route results from integrating *P*Batt over time *t*, whereby *t*Trip denotes the trip time:

$$E\_{\rm Batt} = \int\_{t=0}^{t\_{\rm Trip}} P\_{\rm Batt}(t) \mathrm{d}t \tag{4.20}$$

*Adopted from [79]*. One goal of the ALC is to compute an energy-efficient velocity trajectory. For reducing the energy consumption of a BEV three different cost functions in time domain are compared in [81]:

First, motivated by reports in literature that a velocity profile with low acceleration and deceleration is beneficial for energy savings, a penalization of the absolute value of acceleration is investigated. Due to the resulting reluctance to change velocity limits, this approach is suitable for the considered ACC example only to a limited extent. The approach offers the least energy savings in the comparison (8.4 % and 4.1 % depending on the scenario). This approach is also used in [107] and has the benefit that no power consumption model is needed.

Second, the difference of *P*Batt from its minimum value is penalized, which means that the recuperation maximum is seen as the optimal state. This approach leads to unnecessary strong and frequent recuperations, that are followed by corresponding accelerations. As losses occur in both recuperation and traction mode, this approach is the second least effective (10.6 % and 5.2 % on average).

The highest effectiveness (11.9 % and 5.6 % on average) comes from avoiding deviations of *P*Batt from zero only for the case of traction and to not consider recuperation.

 The rationale of the energy consumption optimization approach in this work is to avoid power losses between battery and wheels denoted by *P*Loss:

$$P\_{\rm Loss} = P\_{\rm Batt} - P\_{\rm trac,mech} \tag{4.21}$$

The corresponding efficiency measure *<sup>P</sup>*trac,mech/*P*Batt is known as *energy efficiency of driving* and *tank to wheel efficiency* [10].

*P*Loss can be expressed with respect to *P*trac,elec as in Figure 4.5. The data points result from computing the driving resistances and power train quantitites

Figure 4.5: Power loss between HV battery and wheels PLoss versus electrical traction power Ptrac,elec for various combinations of vehicle velocity vVhcl and vehicle longitudinal acceleration v˙Vhcl,x. *Adopted from [79].*

for various combinations of vehicle velocity vVhcl and vehicle longitudinal acceleration v˙Vhcl,x with road curvature and slope both equal zero.

*P*Loss increases with the absolute value of vVhcl and v˙Vhcl,x. Arrows indicate how the operating points are shifted when vVhcl and v˙Vhcl,x are increased. The *P*Loss minima for each *P*trac,elec can be approximated by a parabola through the origin. The slope of the parabola increases with the absolute value of *P*trac,elec indicating that the efficiency of the power transmission decreases with increasing amount of transferred power. *Adopted from [79]*.

Sensitivity analyses by [10] reveal that *<sup>P</sup>*trac,mech/*P*Batt is the parameter with the highest impact on the energy consumption in a BEV. Frequently, consumption models that require detailed knowledge of these usually only roughly known parameter values are used at the risk of inaccurate results [10]. To overcome this problem, this work applies data-driven models that learn such relevant parameters in aggregated form during vehicle operation.

 Due to lack of sensors *P*Loss is not known during vehicle operation and therefore cannot be used for a data-driven model. However, *P*trac,elec can be derived from voltage and current sensor data of the power electronics and can serve as a proxy for *P*Loss according to the above considerations. Therefore the energy optimization criterion in the cost function of the ALC is designed to penalize the absolute value of *P*trac,elec. *Adopted from [79]*.

In comparison, the approach of this work comes closest to the third cited method but goes beyond in that it additionally penalizes the also imperfect recuperation mode. Furthermore, using *P*trac,elec as optimization criterion is assumed to be more effective than using acceleration and deceleration as the first cited method does. This is because for the same requested acceleration or deceleration the required *P*trac,elec can differ significantly depending on road slope and vehicle velocity.

 The power required for auxilaries such as light, ventilation, heating, cooling and radio can contribute to the total energy consumption for a trip significantly, in case of low velocity <sup>v</sup>Vhcl <sup>&</sup>lt; <sup>30</sup> km/h. For <sup>v</sup>Vhcl <sup>&</sup>gt; <sup>80</sup> km/h the auxiliary power demand is negligible. It also strongly depends on environmental conditions and individual comfort preferences. Its 5 % and 95 % quantils are given by 0.2 kW and 1.3 kW [10]. *Adopted from [79]*.

Furthermore, some components such as the AC compressor are only temporarily active and their activation can be scheduled. Therefore a predictive operating strategy that consideres both traction power demand and cooling power demand has much more energy-saving potential compared to for instance penalizing the sum of instantaneous AC power consumption and electrical traction power. A longitudinal control that includes operating strategies of auxiliaries is presented in [183].

 For these reasons this work focuses on optimization of traction consumption and neglects auxiliaries by assuming *P*Batt,dem = *P*trac,elec. Even without explicit consideration of power demand of auxiliaries, the penalization of *P*trac,elec has a beneficial effect on efficiency because any additional consumption of auxiliaries further increases the HV battery power loss.

The goal is to penalize *P*trac,elec only to such an extent, that the ALC avoids inefficient power peaks that have negligible effect on the trip time. *Adopted from [79]*. With large penalty for *P*trac,elec, the vehicle will drive very slow or even come to a standstill because of lack of *P*trac,mech that is needed to overcome the driving resistance. Then the resulting trip time will strongly increase as well as the energy consumption because of the power demand of auxiliaries. However such large penalty for *P*trac,elec is far beyond the driver acceptance.

# **4.5 Vehicle models for simulation environment**

The preceding sections investigated the driving resistances that result from a certain vehicle velocity vVhcl, vehicle longitudinal acceleration v˙Vhcl,x, road slope angle α and road curvature κ and stated how the corresponding HV battery power *P*Batt can be derived from the mechanical traction power *P*trac,mech. The upper diagram in Figure 4.6 depicts this approach, which is referred to as *backward simulation*. Starting from the desired driving state backward simulation computes the state of components such as gear box against the direction of the effect chain.

Backward simulation assumes that the power train can provide the power needed for the given driving situation. Due to the missing consideration of physical root causes, this approach is also denoted *non-causal modelling* [45]. Non-causal models include no control loop, therefore no controller is needed. However, backward simulations are only suitable for quasi-stationary simulations that do not take into account transient effects. Quasi-stationary states of components are usually derived by interpolation between values of look-up tables as mentioned in Section 4.3.

Backward simulation will be applied for computing the resulting energy consumption for planned trajectories in Chapter 5 and for recorded drives in Chapter 6. The model will be referred to as *open-loop reference model*.

Figure 4.6: Sequences of forward (top) and backward simulation (bottom).

The equations from the previous sections can also be rearranged for a *forward simulation*. The lower diagram in Figure 4.6 depicts this case. Forward simulation computes component states starting from the cause in accordance with the cause-and-effect chain. Therefore this approach is denoted *causal modelling*.

Forward simulations enable dynamic simulations that incorporate dynamic component behavior, e.g. transient effects in the motors and battery. Components can be described using differential equations which are connected to each other according to the cause-and-effect chain. In this work the cause is the desired motor torque *T*des while v˙Vhcl,x is at the end of the cause-and-effect chain. v˙Vhcl,x is derived by rearranging (4.9) and used for computing vVhcl for the next simulation time step. Causal models include a closed control loop and therefore require a controller.

Forward simulations will be used for testing the interaction between ALC and vehicle. The model will be referred to as *closed-loop reference model*. The controller inside the ALC chooses *T*des such that deviations of vVhcl and v˙Vhcl,x from the desired velocity vdes and the desired acceleration *a*des, respectively, ideally vanish. vdes and *a*des are specified by the planned trajectory of the ALC [48], [62, pp. 37-41].

Figure 4.7: Course of the Weissach route (WR) that is used as reference route for the evaluation of the automated longitudinal control. The dotted arrow indicates the driving direction and the straight line the start and end point of the WR.

## **4.6 Model of reference route**

 The reference route is a roughly 23 km long circuit around the village Weissach in Southwest Germany, hence denoted Weissach route (WR). Figure 4.7 depicts on a map the WR, that comprises urban sections as well as country roads. The legal speed limit varies between 30 km/h and 100 km/h and the road slope ranges from -8 % to 10 %. *Adopted from [79]*.

Figure 4.8 displays the corresponding map data consisting of legal speed limit, road curvature and road slope as well as the therewith derived elevation profile. The map data was extracted from a navigation system and will be used in backward and forward simulations with the closed-loop and open-loop reference model, respectively.

Figure 4.8: Route data of Weissach route (WR) consisting of legal speed limit, road curvature and road slope used for simulation as well as the therewith derived elevation profile. The position is measured from the start of the WR (c.f. Figure 4.7).

# **4.7 Vehicle models for automated longitudinal control**

 This section presents two models for use within the ALC system. For a given driving situation, the first model provides the required traction force *F*trac and the second one the required electrical traction power *P*trac,elec. Both quantities depend on the driving resistances, which in turn are affected by the given driving situation as well as parameters. *Adopted from [79]*.

According to sensitivity analyses in [10], the second most relevant parameter for the accuracy of a vehicle model is *f*<sup>r</sup> . In the comparison, the model is only affected by <sup>ρ</sup> for <sup>v</sup>Vhcl <sup>&</sup>gt; <sup>100</sup> km/h, whereas *<sup>c</sup>*<sup>w</sup> and *<sup>A</sup>* are negligible in the investigated situations. *m*vhcl is only relevant for hilly trips and trips that lead to higher elevation.

 During vehicle operation these parameters are not exactly known. *m*vhcl changes with additional passengers or luggage, *c*<sup>w</sup> and *A* when the convertible top is being opened and *f*<sup>r</sup> with tire temperature.

Therefore adaptive models are applied, which estimate these parameters during vehicle operation either explicitly or in aggregated form. In order to be able to adapt a model, its inputs and outputs must be quantities that can be derived from signals on the Controller Area Network (CAN) bus. *Adopted from [79]*. However, only few quantities of the power train that are relevant for this purpose, are measured. Therefore the adaptive models represent the power train properties only on an aggregated level and cannot state losses in individual components, e.g. gear box losses.

Compared to the vehicle reference models for the simulation environment both vehicle models for ALC are simplified in that they first do not take into account road curvature explicitly and in that they second do not distinguish between vehicle mass and rotational masses of the power train as (4.9) does.

Some publications take into account the inertia of rotating parts of the power train [96, 188] for more accuracy but the majority does not [12, 91, 102, 127, 195, 204]. Due to lack of a fly wheel, less complicated gear boxes and smaller motors, the inertia of rotating parts are usually less in a BEV than in a conventionally driven vehicle. As the inertia of rotating parts is usually not known and elaborate to determine, publications that consider it, mostly increase the vehicle mass by 5%. In the research vehicle, the equivalent masses add up to roughly 5.6 % of the vehicle mass. Varying the mass factor between 0 % and 5 % had almost no impact on the results in the sensitivity analyses for a BEV in [10].

 The simplification regarding rotational masses allows using the sensor longitudinal acceleration *a*<sup>x</sup> as a model input. *a*<sup>x</sup> is measured by the acceleration sensor and is influenced by both change of velocity and road slope angle:

$$a\_{\mathbf{x}} = \dot{\nu}\_{\text{Vhcl,x}} + \mathbf{g} \cdot \sin(\alpha) \tag{4.22}$$

*Adopted from [79]*. The required traction force and the required electrical traction power can differ for the same *a*<sup>x</sup> even if all other influence factors are kept constant. This is because in the case that *a*<sup>x</sup> results from driving at a slope with constant velocity, no rotational acceleration of the power train occurs but it does if *a*<sup>x</sup> results from change of velocity at zero slope. The adaptive models cannot distinguish these cases. The simplification using *a*<sup>x</sup> is frequently used for driving resistances parameter estimation [140, 170, 178].

A comprehensive review of vehicle energy consumption models including an analysis of influence factors on the energy consumption is given in [118]. Furthermore, [118] classifies consumption models into white-box, gray-box and black-box models. White-box models are based on high knowledge of the underlying system and incorporate detailed descriptions of subsystems. In contrast, black-box models only learn an input-output pattern from provided data and require no system knowlege. Gray-box models partly use system knowlege and are partly driven by data.

Based on the inputs to a black-box consumption model, [118] distinguishes engine-based consumption models with engine torque and engine speed as inputs, vehicle-based consumption models with instantaneous vehicle speed and acceleration as inputs and modal-based consumption models which use operating modes such as idling, accelerating or cruising as inputs.

Based on the criteria in [118], the proposed traction force model can be regarded as a gray-box model and the proposed model for the electrical traction power as a vehicle-based black-box consumption model.

Similar models can be found in literature. For example, [48] models the tractive force without cornering resistance and the mechanical traction power of a BEV similar to (4.11) and (4.12) depending on instantaneous speed, acceleration and road grade information. The driving resistance parameters are assumed to be known and constant. A parameter that describes the efficiency of recuperation is estimated with the LS method.

In contrast, [51] takes a more aggregated approach and models the traction force with a quadratic polynomial as a function of vehicle velocity. This traction force model is then used for a multivariate model of power demand and energy consumption of a BEV that depends on vehicle velocity and acceleration. In [194] the polynomial regression model additionally uses the SOC as an input. Two KAF algorithms are compared to RLS for representing *P*trac,elec depending on *a*<sup>x</sup> and velocity in [146].

### **4.7.1 Adaptive traction force model**

 The ATFM answers the question how much traction force is needed in order to fulfill the driving demand of the longitudinal control in the current situation. The ATFM is used as a pilot control in the controller depicted in the lower diagram in Figure 4.6. The controller computes a motor torque demand such that the vehicle tracks the planned velocity trajectory. The ATFM is based on the following simplification of the longitudinal traction force equation (4.11)

that neglects the cornering resistance (4.6) and the equivalent masses of the power train in the inertial force (4.9):

$$\begin{aligned} F\_{\text{trac}} &= F\_{\text{roll}} + F\_{\text{inerr,sampling}} + F\_{\text{cl}} + F\_{\text{air}} \\ &= m\_{\text{Vhcl}} \cdot g \cdot f\_{\text{r}} \cdot \cos(\alpha) + m\_{\text{vhcl}} \cdot \dot{\nu}\_{\text{Vhcl,x}} \\ &+ m\_{\text{vhcl}} \cdot g \cdot \sin(\alpha) + \frac{\rho}{2} c\_{\text{W}} \cdot A \cdot \nu\_{\text{Vhcl}} \, ^2 \end{aligned} \tag{4.23}$$

For small slope angles cos(α) <sup>≈</sup> <sup>1</sup> applies. Therefore the first summand can be approximated by the rolling resistance constant *F*<sup>0</sup> when additionally the vehicle mass in this summand is assumed to change only slowly. Moreover, the second and third summand are merged using (4.22). The traction force *F*trac is not measured but it can approximately be computed as

$$F\_{\rm trac} = T\_{\rm EM,FA} \cdot \frac{i\_{\rm G}}{r\_{\rm dyn,FA}} + T\_{\rm EM,RA} \cdot \frac{i\_{\rm G}}{r\_{\rm dyn,RA}},\tag{4.24}$$

whereby the braking torques in the power train are neglected in contrast to (4.16). *T*EM,FA and *T*EM,RA are no measured quantities either. These signals are computed by the motor ECU from the measured motor voltages and currents using look up tables.

With these adaptions (4.23) in matrix form reads

$$F\_{\rm tra} = \underbrace{(1, a\_{\rm x}, \nu\_{\rm Vhcl})}\_{=:C\_{\rm Vhcl}} \cdot \underbrace{(F\_0, m\_{\rm vhcl}, (\frac{\rho}{2} c\_{\rm w} A))}\_{=:x\_{\rm vhcl}}^{\top} ,\tag{4.25}$$

whereby CVhcl is the vehicle motion vector. The vehicle parameter vector xVhcl summarizes the driving resistance parameters. These parameters are generally not fully known. Therefore xVhcl needs to be estimated such that the estimation minimizes the residual between the traction force computed with (4.24) and the traction force according to the model output from (4.25). *Adopted from [79]*.

This optimization problem is solved with a KF (c.f. Subsection 3.3.2) and the following model equations:

<sup>x</sup>Vhcl,<sup>p</sup> <sup>=</sup> <sup>x</sup>Vhcl,p−<sup>1</sup> <sup>+</sup> <sup>ω</sup><sup>p</sup> (State equation) (4.26) *<sup>F</sup>*trac,<sup>p</sup> <sup>=</sup> <sup>C</sup>Vhcl · <sup>x</sup>Vhcl,<sup>p</sup> <sup>+</sup> <sup>υ</sup><sup>p</sup> (Measurement equation) (4.27)

*<sup>p</sup>* denotes the time step, ω the process noise and υ the measurement noise. Frequently, algorithms such as RLS, KF, EKF are used to determine the driving resistance parameters [140, 170, 178]. For the ATFM a Stenlund-Gustafsson M-Kalman filter described in [145] is applied. It uses a regularization from Stenlund and Gustafsson [168]. This regularization method sets lower bounds for the covariance matrix in such a way that the estimated parameters are kept constant in phases of low excitation, i.e. when *F*trac and CVhcl temporally remain constant, such as while driving with roughly constant velocity and road slope. Low excitation also applies to vehicle standstill but for this phase a simple criterion for pausing the adaption can be defined. *Adopted from [79]*. The ATFM was created by F. Bleimund [22, pp. 24-29] within *e-generation* and reused by the author of this work without modifications.

Literature also proposes methods that focus on fusing the information of different sensors [74, 198] as well as methods that divide the estimation into several stages [101, 199].

### **4.7.2 Adaptive electrical power model**

#### **Model interfaces and features**

 The AEPM answers the question how much electrical traction power *P*trac,elec is required to fulfill the driving demand. The ALC uses the AEPM during trajectory optimization in order to derive trajectories that require few electrical traction power. As shown in Section 4.4, this corresponds to a low power loss between HV battery and wheels.

*P*trac,elec can be determined from the measured voltages and currents of the electric motors that are available on the CAN bus. Apart from the power losses in the drive train, *P*trac,elec equals the mechanical traction power *P*trac,mech which can be computed as the product of traction force *F*trac and vehicle velocity vVhcl (c.f. (4.12)). According to Subsection 4.7.1, *F*trac can be modelled as a function of the CAN signals vVhcl and *a*x. Therefore the AEPM relates vVhcl and *a*<sup>x</sup> to *P*trac,elec:

$$P\_{\rm AEPM} = \text{AEPM}(\nu\_{\rm Vhcl}, a\_{\rm x}) \tag{4.28}$$

#### **Features of power consumption representation**

A substantial fraction of *P*trac,mech is needed to overcome the acceleration resistance *m*vhcl · v˙Vhcl,x from (4.9). The corresponding acceleration power *m*vhcl · v˙Vhcl,x ·vVhcl depends on an unseparable product of v˙Vhcl,x and vVhcl. For negligible road slope angle α these quantities are similar to the model inputs because of (4.22) (c.f. [51]). Therefore the linear model structure used in Subsection 4.7.1 for the ATFM is not suitable for the AEPM. *Adopted from [79]*.

The power consumption representation in time domain is highly nonconvex, as stated at the example of a *<sup>P</sup>*Batt <sup>=</sup> *<sup>f</sup>* (*F*trac, <sup>v</sup>Vhcl) representation in [81]. An approximation of *P*Batt with a convex function can be used as a cost function in an convex optimization problem that can be solved more easily than a nonlinear optimization problem. The quadratic approximation accuracy for various representations is also investigated in [81]. Due to the less prominent nonconvexity of the representation in space domain, the highest accuracy is observed for the (*P*Batt/vVhcl) <sup>=</sup> *<sup>f</sup>* (*F*trac, <sup>v</sup>Vhcl 2 ) representation, which is the energy consumption in Watt seconds per meter. In space domain, however, vehicle dynamics and the vehicle control problem are more nonlinear [81].

#### **Algorithm for model adaption**

 The AEPM is based on the Fixed-Budget KRLS (FB-KRLS) algorithm described in [180], which is a representative of KAFs introduced in Section 2.4. The source code of FB-KRLS is available at [181]. With each new data point, FB-KRLS adds a new kernel support vector in the vVhcl − *a*<sup>x</sup> plane, updates the approximation and discards the least important kernel support vector to keep the required memory constant. During model evaluation the model output *<sup>P</sup>*AEPM results from computing the sum of all kernels at (vVhcl, *<sup>a</sup>*x), weighted with their corresponding control points. *Adopted from [79]*.

The model captures aggregated power train losses that are constant or depend on vVhcl or *a*x. If vehicle parameters or power train properties change, the AEPM will adapt itself accordingly.

While [81] approximates the nonlinear structure by a convex function to get a convex optimization problem, a KAF approximates the nonlinear structure using a nonlinear function and still offers a convex optimization problem for model adaption. For this kind of convex problems there are solvers that converge in a defined time period, which is important for real-time model adaption.

RLS and two fixed-budget KAFs, called KRLS Tracker (KRLS-T) and QKLMS-FB, are applied to (4.28) in a very similar approach in [146]. KRLS-T achieves the highest accuracy, followed by QKLMS-FB. As expected, the linear RLS, proves not suitable for strong nonlinearity and achieves the lowest accuracy.

All three mentioned KAF use the Gaussian kernel defined in (2.21). The KRLS-T algorithm published in [179] in 2012 is a KAF algorithm for time-varying regression that includes a forgetting factor that can handle non-stationary scenarios. In contrast, the FB-KRLS published in 2010 stems from a sliding-window approach with the improvement that it maintains not the kernels for the latest but for the most important data. Its forgetting mechanism is not optimized towards non-stationary scenarios [181].

The idea to model the power train characteristics with a FB-KRLS originates from S. Rhode [146], who also provided a first script in MATLAB that performs this task. Based on this, both researchers continued the investigations independently from each other, so details differ. The content of the remainder of this subsection, except from the normalization described below, stems from the author of this work. The novelity of this work regarding FB-KRLS is limited to the application of this kind of black-box model for trajectory optimization and within the ALC along with required problem-specific adaptions such as the data shift as well as the determination of model hyperparameters, both also mentioned below.

#### **Data transformation**

 For various (vVhcl, *<sup>a</sup>*x) combinations the trajectory optimization algorithm evaluates the AEPM and takes decisions based on the AEPM outputs. The standard FB-KRLS output is zero in border areas where no data points have occured yet. This means that if the trajectory optimization evaluates the model for high velocities or accelerations that are beyond the capabilities of the vehicle, the AEPM states *P*trac,elec ≈ 0. To avoid that results of evaluations of the model in its border areas appear as efficient and therefore favorable driving states to the trajectory algorithm, the model outputs must have large absolute values in border areas. This can be achieved by shifting *P*trac,elec in training data by subtracting a large offset value.

After the shift, all components of training data (vVhcl, *<sup>a</sup>*x, *<sup>P</sup>*trac,elec) and evaluation data (vVhcl, *<sup>a</sup>*x) are additionally scaled to a ranges between -1 and 1 by multiplying each of them with a constant between 0 and 1. This process is called normalization. Frequently better aproximation quality is observed for models that work with normalized data.

Shift and normalization form the transformation. Results of model evaluations are retransformed by a denormalization and backshift operation before they are provided to the trajectory optimization algorithm.

Figure 4.9: Electrical power PAEPM according to the adaptive electrical power model (AEPM) depending on sensor longitudinal acceleration a<sup>x</sup> for various vehicle velocities vVhcl. The gray shaded area is the ALC operating area, max Ptrac,elec the maximum traction power and min Ptrac,elec the maximum recuperation power. *Adopted from [79].*

Figure 4.9 depicts the retransformed model output as a function of *a*<sup>x</sup> for various nonnegative vehicle velocities. Negative velocities are not shown for simplicity and because with ALC the vehicle only drives forward. For medium negative *a*<sup>x</sup> the *P*trac,elec is negative indicating that power is recuperated into the battery. The gray shaded area indicates possible operating points of the vehicle while driving with activated ALC under the assumption of no slope. As the ALC limits are defined with respect to v˙Vhcl,x, the width of the area will change in presence of slope according to (4.22).

In border areas that exceed the vehicle capabilities the shift leads to very large AEPM outputs of up to three times the maximum *P*trac,elec. As a result, the trajectory optimization algorithm avoids trajectories that include such unreachable (vVhcl, *<sup>a</sup>*x) combinations. *Adopted from [79]*.

In case of negative slope peaks or strong braking, the simple shift leads to a zero-crossing of *<sup>P</sup>*trac,elec at roughly <sup>−</sup>7 m/<sup>s</sup> <sup>2</sup> <sup>≤</sup> *<sup>a</sup>*<sup>x</sup> ≤ −5 m/<sup>s</sup> <sup>2</sup> before *P*trac,elec increases to large values. This case can still mislead the trajectory optimization algorithm but for the given setup it did not present a problem. As Figure 4.8 indicates, on the WR peaks in the course of the road slope are stronger in the positive direction than in the negative direction. This asymmetrical road slope distribution causes that in AEPM evaluations the maximum absolute value of *<sup>a</sup>*<sup>x</sup> is greater for *<sup>a</sup>*<sup>x</sup> <sup>&</sup>gt; <sup>0</sup> than it is for *<sup>a</sup>*<sup>x</sup> <sup>&</sup>lt; <sup>0</sup> and that border areas on the left hand side of Figure 4.9 are not reached.

To prevent that negative slope peaks cause issues on other hilly routes, the shift should be improved by subtracting a plane from the model input data instead of a constant. This plane can be designed so that it goes through the origin and is tilted upwards in the directions of the vVhcl and *a*<sup>x</sup> axes, e.g. <sup>−</sup>*c*<sup>1</sup> · <sup>v</sup>Vhcl <sup>−</sup> *<sup>c</sup>*<sup>2</sup> · *<sup>a</sup>*<sup>x</sup> <sup>+</sup> *<sup>c</sup>*<sup>3</sup> · *<sup>P</sup>*trac,elec <sup>=</sup> <sup>0</sup> with constants *<sup>c</sup>*1, *<sup>c</sup>*2, *<sup>c</sup>*<sup>3</sup> <sup>≥</sup> <sup>0</sup>. By choosing these constants appropriately, physically unreasonable zero-crossings of *P*AEPM can be avoided.

#### **Determination of model hyperparameters**

The question is how kernel variance σ 2 in (2.21) and kernel count *M* should be chosen for the AEPM. For better differentiation from other parameters like control points, σ 2 and *M* are called hyperparameters [146] and determining them is part of the model selection process [72, pp. 128-130]. A model with many parameters usually provides more accurate results at the cost of increased effort for parameterization and evaluation.

Model selection criteria, also called *information criteria*, assess models and thereby can help to find a trade-off between model complexity and model quality. The general structure of an information criterion is:

$$\text{criterior} = P \cdot \ln\left(\frac{SSE}{P}\right) + \text{penalty term} \tag{4.29}$$

ln(·) is the natural logarithm, *P* the number of observations and SSE is the sum of squared errors with

$$SSE = \sum\_{p=1}^{P} \left(\mathbb{y}\_p - \mathbb{y}\_p\right)^2. \tag{4.30}$$

y denotes the measurement and yˆ the model output. The selected criterion is computed for a set of trained models and the model with lowest criterion value is chosen. Several information criteria have been introduced, which are based on different principles and differ in the penalty term. In a comparison in [89] the Hannan-Quinn information criterion (HQC) performs best:

$$\text{HQC} = P \cdot \ln\left(\frac{SSE}{P}\right) + M \cdot \ln\left(\ln(P)\right) \tag{4.31}$$

Number of model parameters *<sup>M</sup>* and kernel variance σ 2 from (2.21) span the hyperparameter space for the search for the most suitable AEPM. For each combination of *<sup>M</sup>* <sup>=</sup> <sup>10</sup>, <sup>20</sup>, . . ., <sup>100</sup> and σ <sup>2</sup> <sup>=</sup> <sup>1</sup>/1, 1/2, . . ., 1/<sup>30</sup> a model is trained and tested to determine the model with the lowest HQC.

Data for model training and test was created using the open-loop reference model. Thereby the vehicle longitudinal acceleration was sampled with v˙Vhcl,x = <sup>−</sup>10, <sup>−</sup>9.8, . . ., 10 m/<sup>s</sup> 2 and the vehicle velocity with <sup>v</sup>Vhcl <sup>=</sup> <sup>0</sup>, <sup>0</sup>.5, . . ., 56 m/s. Slope and curvature were assumed equal zero. Operating points that are beyond the vehicle capabilities according to the open-loop reference model were removed from the data set. The data set that comprises all possible vehicle operating points (PVOP) in random order is split into a PVOP training data set and a PVOP test data set. The randomly chosen PVOP test data set comprises 10 % of the original PVOP data set and the remaining data points form the PVOP training data set.

Additionally, a WR data set was created from the closed-loop reference model output during a simulated drive with the ALC on the WR including slope and road curvature information. The WR data set was split analogously into WR training data set and WR test data set. PVOP data set and WR data set contain roughly the same amount of data points.

The upper diagram of Figure 4.10 depicts the best HQC as well as the corresponding σ 2 for each investigated kernel count. Only for models that are both trained and tested using data from the PVOP data set the HQC course shows a local minimum at 50 kernels along with comparatively large kernel variance values.

The lower diagram of Figure 4.10 depicts the corresponding 95 % quantile *Q*95% of absolute values of the relative error *e*rel between the output of the AEPM and the reference model for PVOP and WR, respectively. With increasing *M* the 95 % quantiles decrease.

Models that are both trained and tested with PVOP data achieve better HQC values and lower quantiles than models that are trained with PVOP data and tested with WR data. The PVOP data set contains much more dynamic driving states than the WR. Furthermore, only the PVOP test data set evaluates a model that has been trained with the PVOP training data set in its border areas. Therefore, the worse result for the WR test data set seems counterintuitive at first.

However, only the WR data set considers road curvature and road slope. On the WR the road slope is up to 10 %. The AEPM does not use road curvature input and summarizes road grade and change of vehicle velocity in the input *a*x. Hence, the results indicate that road geometry has more influence on the quality of the model output than the driving style. However, for *M* ≥ 40 the quantils are below 2 % for all considered combinations of training and test data sets. Therefore not considering road curvature in the AEPM explicitly is acceptable.

Since the goal is not to optimize the model parametrization with respect to the WR but to find a suitable model for the vehicle in general, the best model parametrization according to HQC for the PVOP test set was chosen. This is a model with *<sup>M</sup>* <sup>=</sup> <sup>50</sup> and σ <sup>2</sup> <sup>=</sup> <sup>1</sup>/<sup>9</sup> that achieves *<sup>Q</sup>*95%(|*e*rel|) <sup>=</sup> <sup>0</sup>.94 % for

Figure 4.10: Results of full factorial search for best model hyperparameters kernel count M and kernel variance σ<sup>2</sup> . Top: Both the best HQC and the corresponding σ<sup>2</sup> are plotted versus M for each combination of training and test using the data sets from possible vehicle operating points (PVOP) and Weissach route (WR) . Bottom: 95 % quantile Q95% of absolute values of relative error erel between output of AEPM and reference model versus M.

PVOP and *<sup>Q</sup>*95%(|*e*rel|) <sup>=</sup> <sup>1</sup>.5 % for WR. This AEPM model is used in the remainder of this work and depicted in Figure 4.9.

With the hyperparameter optimization technique from [146] the KRLS-T accuracy increases for *M* ≤ 50 and converges for more kernels. Therefore *M* = 50 is considered the optimal KRLS-T hyperparameter, analogously to this work. However, with σ <sup>2</sup> <sup>=</sup> <sup>6</sup>.<sup>25</sup> the reported kernel variance is different. Reasons can be contrasting data sets and differences in the KAF algorithms themselves.

# **5 Planning of velocity trajectories**

This chapter presents a trajectory planning approach based on the B-spline approximation methods from Chapter 3. Section 5.1 states how an upper speed limit for the route ahead is created from map data. Section 5.2 includes considerations on how to represent the planned vehicle velocity and Section 5.3 describes the trajectory optimization. Section 5.4 proposes an extension of the trajectory optimization problem for taking into account the required electrical traction power. Section 5.5 states ways to enforce constraints on the trajectory beginning and Section 5.6 summarizes the scientific contribution of this work regarding trajectory planning.

## **5.1 Generation of upper speed limit**

An upper speed limit for each meter of the road section ahead of the vehicle is computed using map data. The upper speed limit takes into account legal restrictions, limitations from driving dynamics as well as safety and comfort requirements and serves as input data for the trajectory optimization process.

The map data consists of three *P* × 1 vectors, each with component index *<sup>p</sup>* <sup>=</sup> <sup>1</sup>, . . ., *<sup>P</sup>*. The vectors state the meter-discrete courses of road slope γ, road curvature <sup>κ</sup> and legal speed limit <sup>v</sup>Lim,Law for the road section ahead of the vehicle in driving direction. The first component of each vector refers to the current vehicle position. *P* is called the *length of the electronic horizon*.

Figure 5.1 depicts various speed limits that occur during generation of the upper speed limit and are described in the following paragraphs.

Figure 5.1: Computation of upper speed limit with respect to position measured from vehicle at km 11 of Weissach route in driving direction. Corresponding route data depicted in Figure 4.8 between km 11 and km 13. Legal speed limit vLim,Law, legal speed limit including speedometer offset vLim,Speedo, speed limit resulting from curvature vLim,Curve and speed limit resulting from crests vLim,Crest are intermediate quantities. The speed limit resulting from map data vLim,Map is the minimum of vLim,Speedo, vLim,Curve and vLim,Crest. End result is the speed limit from map data with desired acceleration vLim,Map,v˙ .

The course of the legal speed limit vLim,Law forms the basis of the upper speed limit. The speedometer in a vehicle includes an offset and therefore indicates a velocity that is slightly higher than the actual vehicle velocity. For better acceptance by the driver, the legal speed limit including speedometer offset vLim,Speedo is used instead of vLim,Law. This causes that the speedometer will indicate roughly the same velocity value as the traffic sign when there are no relevant restrictions other than vLim,Law present.

Due to tight curves, crests or comfort requirements the upper speed limit needs to be corrected starting from vLim,Speedo further downwards.

Driving with vehicle velocity <sup>v</sup>Vhcl on a road with curvature <sup>κ</sup> causes a lateral acceleration v˙Vhcl,y = v 2 Vhcl · <sup>κ</sup> (c.f. (4.5)). A maximum absolute value of lateral acceleration v˙Vhcl,y,max is specified, which leads to the speed limit resulting from curvature vLim,Curve given by

$$\nu\_{\text{Lim,Curve}} \le \sqrt{\frac{\dot{\nu}\_{\text{Vch,y,max}}}{|\kappa|}}. \tag{5.1}$$

Thereby | · | denotes the absolute value. A comfortable lateral acceleration maximum varies with vehicle speed [153]. As a first draft a lateral acceleration table v˙Vhcl,y,max(v) was created using a characteristic curve from [153]. In test drives the characteristic curve was further adapted to suit the vehicle characteristics. The lateral acceleration table consists of <sup>v</sup>˙Vhcl,y,max, <sup>v</sup> supporting points because these quantities can be specified more conveniently than κ in test drives. With (4.5) the corresponding κ values are computed. Thereafter <sup>v</sup>˙Vhcl,y,max for a given <sup>κ</sup> is determined from map data by linear interpolation and vLim,Curve is derived using (5.1).

Crests on the route cause another restriction on the upper speed limit. From road slope data an elevation profile is obtained and it is calculated how far ahead the road can be seen according to geometrical considerations. It needs to be ensured that the vehicle can always come to a standstill within the sighting distance ∆*s*Sighting. Assuming that the absolute value of the maximum deceleration is the gravitational constant g leads to the speed limit resulting from crests vLim,Crest given by vLim,Crest ≤ p 2 · ∆*s*Sighting · g.

The speed limit resulting from map data vLim,Map is the minimum of vLim,Speedo, vLim,Curve and vLim,Crest. Usually vLim,Crest is high compared to the other quantities and therefore rarely determines vLim,Map.

In the last step the desired longitudinal acceleration *a*des is taken into account. The desired acceleration and deceleration values are derived by interpolating with respect to velocity v in separate acceleration tables for acceleration *<sup>a</sup>*des,pos(v) and deceleration *<sup>a</sup>*des,neg(v) < <sup>0</sup>. Enforcing the constraints

$$\begin{aligned} \upsilon\_{p+1} &\le \sqrt{\upsilon\_p + 2 \cdot \Delta s \cdot a\_{\text{des,pos}}(\upsilon\_p)}, \ p = 1, 2, \dots, P - 1 \\ \upsilon\_{p-1} &\le \sqrt{\upsilon\_p - 2 \cdot \Delta s \cdot a\_{\text{des,neg}}(\upsilon\_p)}, \ p = P, P - 1, \dots, 2 \end{aligned} \tag{5.2}$$

with ∆*s* = 1 on vLim,Map gives the speed limit from map data with desired acceleration vLim,Map,v˙ , which is the output of the whole procedure.

The content described above in this section originates from F. Bleimund [22, pp. 29, 30], who implemented it during *e-generation*. The adaptions done during *e-volution* are limited to enhancements of the acceleration tables and adaptions of their entries during test drives.

Other works such as [114, 139] additionally specify a lower speed limit as a certain percentage of the upper speed limit. Both limits then form a so-called *driving tube* that defines the solution space of valid trajectories. An alternative term is *driving envelope* [44]. This work specifies no explicit lower limit but enforces a nonnegativity constraint on velocity trajectories as Section 5.5 will state.

## **5.2 Representation of vehicle velocity in time domain**

There are several ways to define a course of velocity that stays below the upper speed limit. Figure 5.1 illustrates that in general only a functional representation with many degrees of freedom can sufficiently adapt to the upper speed limit over several kilometers.

Section 2.3 investigated the properties of several function types and the advantages of spline functions over polynomials. Based on the conclusions drawn there, the velocity trajectory will be represented using a cubic B-spline function. This function is twice continuously differentiable which means the trajectory velocity vTJY is smooth. This is beneficial for driving comfort but not sufficient for jerk-free driving behavior. If the B-spline function has many degrees of freedom in a short time period and unfavorably chosen control points, it can lead to an oscillating and uncomfortable velocity course.

A further helpful feature is that in each interval a B-spline function lies within the convex hull of the control points that are relevant for this interval (c.f. Section 2.3 and Section 3.1). This feature allows to enforce vTJY ≥ 0 by simply restricting the control points to nonnegative values.

Since the upper speed limit depends on the position, a straight-forward approach is to define the trajectory with respect to the position as well. This was done in [22, 139].

RBA described in Subsection 3.3.3 allows to adapt the control points of a Bspline function iteratively such that it approximates a set of data points in the WLS sense. With RBA a spatial velocity trajectory vTJY(*s*) that depends on the position *s* can be adapted to the upper speed limit by solving the LS problem

$$\hat{\mathbf{x}} = \operatorname\*{arg\,min}\_{\mathbf{x}} \sum\_{p=1}^{P} \left( \nu\_{\text{Lim,TIY},p} - \nu\_{\text{TIY}}(\mathbf{s}\_p) \right)^2 \tag{5.3}$$

using the data set

$$(s\_p, \nu\_{\text{Lim,TIV},p}), \ s\_p = p-1, \ p = 1, 2, \dots, P,\tag{5.4}$$

in which <sup>v</sup>Lim,TJY,<sup>p</sup> is derived from <sup>v</sup>Lim,Map,v˙ as (5.12) will define. A spatially defined trajectory can easily be compared against the upper speed limit and adjusted downwards at any position without risking to violate the upper speed limit at another position. However, the spatial trajectory definition has at least four flaws:

First, at low speeds the position *s* changes slowly in a given time interval. If the knots of vTJY(*s*) are spatially equidistant, the trajectory can only represent a comparatively reluctant driving behavior at low speeds, whereas at highway speeds it has an excessive number of degrees of freedom in each time interval that facilitates undesired velocity oscillations and increases the computational effort unnecessarily.

RBA only chooses the control points and requires knots as an input. Regarding RBA this means that simply specifying spatially equidistant knots is not suitable. This was also confirmed by real test drives with a spatial trajectory planning approach based on RBA. Therefore a knot placement procedure was added that computed spatial knot positions that were equidistant with respect to time. The degrees of freedom were then spatially dense at low speeds and with increasing vTJY their density decreased.

There are also solutions for adapting both knots and control points when fitting a B-spline function to data [36, 93, 130] but this problem is a nonconvex one with many local minima and therefore difficult to solve [15]. For example, [158] solves a reduced nonlinear B-spline LS approximation problem, in which only the knots are optimization variables using a generalized Gauss-Newton method, whereas [141] applies the LM algorithm from Subsection 3.4.1. A review of state of the art methods for this kind of problem is provided by [43].

Second, a spatial velocity trajectory cannot represent driving off again after coming to a standstill because if vTJY(*s* ∗ ) = 0, at any position *s* ∗ , then *s* ∗ cannot be left anymore. Hence, starting from a standstill requires a different trajectory planning approach.

Third, the time *t* ∗ at which a vTJY(*s*) trajectory reaches a certain position *s* ∗ requires to compute the integral *t*(*s* ∗ ) = R s ∗ s=0 1 vTJY(s) d*s*. For a cubic function vTJY(*s*) the solution can be determined using a partial fraction decomposition (PFD). Since the solution of PFD depends on the control point values, a system of equations needs to be solved repeatedly.

Fourth, derivates of vTJY(*s*) are no common quantities with known interpretation. Common quantities refer to time and deriving them from vTJY(*s*) is costly. For example, the derivative vTJY 0 (*s*) is not the trajectory acceleration *a*TJY(*s*) but the pseudo trajectory acceleration [55]. *a*TJY needs to be computed from vTJY(*s*) as a product:

$$a\_{\rm TIV}(s) = \frac{\mathrm{d}\nu\_{\rm TIV}(s)}{\mathrm{d}t} = \frac{\mathrm{d}s}{\mathrm{d}t} \cdot \frac{\mathrm{d}\nu\_{\rm TIV}(s)}{\mathrm{d}s} = \nu\_{\rm TIV}(s) \cdot \nu\_{\rm TIV}(s) \tag{5.5}$$

Hence, *a*TJY cannot be repesented by the linear combination of basis functions and control points from (2.17) and (3.3), respectively, anymore. Equation (5.5) gives oscillations in the acceleration course, which become larger as the knot distance is decreased to allow for more agile vehicle behavior. The reason is that the result of (5.5) is no B-spline product and therefore also does not fulfill the convex hull property. Ways to correctly compute product functions of B-spline functions are given in [121, 133]. The approach in [133] relies on intermediate transformations to the Bézier functions (c.f. Section 2.3). A sliding window B-spline multiplication algorithm is presented in [30].

A time dependent velocity trajectory overcomes these flaws. First, the knots can simply be chosen temporally equidistant. Then the trajectory represents a vehicle behavior whose agility does only depend on the chosen constant knot density but not on vehicle velocity. Second, driving off from a standstill poses no problem because *t* still changes during a standstill and so can vTJY(*t*). Third, the corresponding trajectory position *s*TJY(*t*) is obtained efficiently by integration:

$$\rho\_{\rm TLY}(t) = \int\_{\tau=0}^{t} \nu\_{\rm TYY}(\tau)d\tau \tag{5.6}$$

Fourth, meaningful quantities like trajectory acceleration and trajectory jerk arise from a linear combination of basis functions and control points.

For these reasons a time dependent trajectory vTJY(*t*) is chosen. The problem of adapting vTJY(*t*) to the upper speed limit is formulated analogously to (5.3) as an approximation problem but an additional change of representation space from position to time is included:

$$\hat{\mathbf{x}} = \underset{\mathbf{x}}{\text{arg min}} \sum\_{p=1}^{P} \left( \upsilon\_{\text{Lim,TIIY}} \left( s\_{\text{TIIY}}(t\_p) \right) - \nu\_{\text{TIIY}}(t\_p) \right)^2 \tag{5.7}$$

The trajectory position *s*TJY is given by (5.6) and computed as in (3.10).

In contrast to (5.3), the error vLim,TJY (*s*TJY) − vTJY in (5.7) does not linearly depend on the control point vector x. Instead, x is linked to the error by the nonlinear upper speed limit, which makes (5.7) a NWLS problem.

The described approach is similar to parametric B-spline curve approximation. As mentioned in Section 2.3, a B-spline curve is a generalization of a B-spline function that uses multi-dimensional control points. These enable an independent curve shape in each dimension such that the curve can represent edges of a geometrical object [3]. The velocity trajectory approximates (*s*p, <sup>v</sup>Lim,TJY,p) data points with respect to the parameter *t*. However, *s*TJY and vTJY are coupled via (5.6) and by still using a B-spline function with scalar control points this constraint can be enforced.

A disadvantage of the temporal trajectory definition is that road grade, speed limit and road curvature are position dependent, whereas the position of the vehicle depends on the future course of velocity. Therefore a velocity optimization requires a vehicle position estimate [82]. Specifically for the approach in (5.7) this means that if any component of the solution xˆ of the optimization problem is modified, the trajectory will likely exceed vLim,Map,v˙ at some later point in time. In contrast, with vTJY(*s*) control point values can always be reduced without risking to violate vLim,Map,v˙ afterwards.

For completeness a *s*TJY(*t*) trajectory is considered as well. Against such a trajectory argues the fact that *s*TJY is unbounded and may take large values for long trajectories. This encourages numerical problems and instable computations in optimization algorithms.

## **5.3 Trajectory optimization**

 Trajectory optimization involves adapting the vTJY(*t*) trajectory function to the data set in (5.4) that is created using the upper speed limit vLim,Map,v˙ . vTJY(*t*) is defined according to (3.3) with degree *<sup>d</sup>* <sup>=</sup> <sup>3</sup>, knot vector κ and control point vector <sup>x</sup>. κ given by

$$\begin{aligned} \mathbf{x} &= (\kappa\_1, \kappa\_2, \dots, \kappa\_K) \\ &= (\Delta t\_\kappa \cdot (-d), \Delta t\_\kappa \cdot (-d+1), \dots, \Delta t\_\kappa \cdot (-d+K-1)) \end{aligned} \tag{5.8}$$

has equidistant and strictly monotonously increasing entries. <sup>∆</sup>*t*κ denotes the constant temporal distance of neighboring knots. Due to the choice of κ, the trajectory can be evaluated for *t* ≥ 0.

*t* is discretized by a constant temporal distance of neighboring data points ∆*t*It:

$$
\Delta t\_p = (p - 1) \cdot \Delta t\_{\text{It}}, \; p = 1, \ldots, P \tag{5.9}
$$

The approximation problem that is solved reads

$$\begin{split} \hat{\mathbf{x}} &= \operatorname\*{arg\,min}\_{\mathbf{x}} \sum\_{p=1}^{P} \left( R\_{\mathbf{v}}^{-1} \cdot \left[ \nu\_{\text{Set}} \left( s\_{\text{TOY}}(t\_{p}) \right) - \nu\_{\text{TOY}}(t\_{p}) \right] \right)^{2} \\ &+ R\_{a}^{-1} \cdot \left( a\_{\text{Set}} - a\_{\text{TOY}}(t\_{p}) \right)^{2} + R\_{j}^{-1} \cdot \left( j\_{\text{TOY}}(t\_{p}) \right)^{2} \right) . \end{split} \tag{5.10}$$

*a*TJY denotes the trajectory acceleration and *j*TJY the trajectory jerk. These quantities are the first and second derivative of vTJY and can be calculated according to (3.5). The trajectory position *s*TJY is measured from *t* = 0, hence *s*TJY(*t* = 0) = 0. *s*TJY can be computed using (3.10) and (5.6).

Unless stated otherwise, the set point of trajectory velocity vSet and the set point of trajectory acceleration *a*Set are defined as follows:

$$\nu\_{\text{Set}} = \nu\_{\text{Lin,TJY}}, \ a\_{\text{Set}} = 0 \tag{5.11}$$

In contrast to (5.7), the optimization problem in (5.10) includes two additional summands that refer to the derivatives *a*TJY and *j*TJY of the vTJY(*t*) trajectory. By penalizing deviations of *a*TJY from *a*Set and deviations of *j*TJY from zero, the vTJY(*t*) function is stabilized and uncomfortable driving is avoided, which can be caused by acceleration peaks and velocity oscillations.

Each optimization goal has a corresponding weight. *R* −1 <sup>v</sup> denotes the weight of velocity error square, *R* −1 a the weight of acceleration error square and *R* −1 j the weight of jerk error square. The reciprocals of the weights follow the interpretation of the filter algorithms from Subsection 3.3.2 and Subsection 3.4.2 by referring to the variances of the artificial measurements vSet, *a*Set and 0. *R*<sup>v</sup> is the variance of velocity measurement, *R*<sup>a</sup> the variance of acceleration measurement and *R*<sup>j</sup> the variance of jerk measurement. *R* −1 v can be interpreted as a weight for the optimization goal of few travel time whereas both *R* −1 a and *R* −1 j refer to driving comfort. A suitable weighting combination reads *<sup>R</sup>*<sup>v</sup> <sup>=</sup> <sup>5</sup>, *<sup>R</sup>*<sup>a</sup> <sup>=</sup> <sup>10</sup>, *<sup>R</sup>*<sup>j</sup> <sup>=</sup> <sup>1</sup>. This combination was derived by experiments, validated in real test drives and will be used in the remainder of this work.

WLS and NWLS approximation algorithms usually solve an unconstrained problem and compute a function that is close to the data points. Problem (5.10) follows this approach and penalizes deviations of the function value from the data symmetrically. This interpretation of the data neglects that the data points of the upper speed limit also present a constraint to vTJY. This constraint character is taken into account by using modified data in (5.10). Set point of trajectory velocity vSet in (5.11) is not vLim,Map,v˙ but the speed limit for trajectory optimization vLim,TJY:

$$\nu\_{\rm Lin,TIV} = \min\left(\nu\_{\rm L,in,Map,\psi} \left[p\_{\rm min}\right], \dots, \nu\_{\rm L,in,Map,\psi} \left[p\_{\rm max}\right]\right)$$

$$p\_{\rm min} = \text{round}\left(s\_{\rm TIV} - \nu\_{\rm TIV} \cdot \Delta t\_{\rm L,in,TIV} + 1\right)\tag{5.12}$$

$$p\_{\rm max} = \text{round}\left(s\_{\rm TIV} + \nu\_{\rm TIV} \cdot \Delta t\_{\rm L,in,TIV} + 1\right)$$

vLim,TJY is the minimum of vLim,Map,v˙ within a spatial distance around the calculated trajectory position *s*TJY. The distance depends on the trajectory velocity vTJY and the temporal safety margin to upper speed limit ∆*t*Lim,TJY. ∆*t*Lim,TJY

is a tuning parameter. In combination with ∆*t*It sufficiently small it causes that there are enough data points at local minima of vLim,Map,v˙ such that the trajectory does not exceed local minima of vLim,Map,v˙ .

The nonlinear approximation problem (5.10) can be solved with LM and NRBA. To an approximation of (5.10) RBA can be applied.

#### **Advantage of solving the approximated optimization problem**

Regarding the control, [82] states that the time domain is beneficial because the vehicle dynamics itself is almost linear except from the air resistance. Therefore the problem can easily be simplified to a linear constrained quadratic programming (QP) problem. In contrast, the control in space domain is very nonlinear when the distance to the vehicle ahead is considered and therefore harder to be solved. Due to the spatially defined upper speed limit, (5.10) is a nonlinear problem though.

However, RBA can solve an approximation of the nonlinear problem (5.10) by a quadratic problem. For this, in each iteration *p* the trajectory position *s*TJY at the current time *t*<sup>p</sup> is determined by temporal integration of the trajectory velocity vTJY with its currently estimated control point vector x in order to derive the upper speed limit vLim,TJY(*s*TJY(*t*p)). Then vSet = vLim,TJY is provided to the linear WLS algorithm RBA. The linear algorithm has no knowledge of the nonlinearity and assumes that the error vLim,TJY − vTJY depends linearly on x.

The solution of RBA is not guaranteed to be optimal under these conditions. However, the effort is lower than with a method for NWLS and the solution of the approximated problem is satisfactory compared with an in general not globally optimal solution of the actual NWLS problem. For predictive vehicle control, [47, 28, 86] also approximated a nonlinear problem by a quadratic problem in order to be able to apply a quadratic programming method instead of a sequential quadratic programming method like LM.

#### **Influence of temporal safety margin to upper speed limit**

The upper diagram in Figure 5.2 depicts the velocity v versus the position *s* measured from the vehicle in driving direction. The dashed line shows the speed limit from map data with desired acceleration vLim,Map,v˙ . The solid red line is a trajectory computed using RBA with <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s, <sup>∆</sup>*t*κ <sup>=</sup> 1 s, <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.1 s and *I* = 1, whereby *I* denotes the number of spline intervals. Each red dot is a set point of trajectory velocity vSet that occurs during the iterations of RBA. For better view, only every tenth vSet point is shown in Figure 5.2. The blue dots and lines show the same quantities for ∆*t*Lim,TJY = 4 s.

The temporal parametrization causes that in the v − *s* diagram the vSet data points are less close at higher speeds (e.g. for *s* ≤ 200 m) compared to at low speeds (e.g. around *s* = 600 m). Due to the temporal safety margin to upper speed limit in (5.12), the spatial distance between the courses of vSet and vLim,Map,v˙ is larger at higher speeds. Provided that ∆*t*It is sufficiently small and that ∆*t*Lim,TJY is sufficiently large, there are enough data points to cause that the resulting trajectory does not exceed vLim,Map,v˙ at local minima, e.g. at *s* = 400 m and *s* = 600 m.

Increasing ∆*t*Lim,TJY also reduces the possibility of short-lasting velocity peaks. For example, at *s* = 500 m vLim,Map,v˙ has a local maximum that both vSet with ∆*t*Lim,TJY = 1 s and the corresponding trajectory vTJY(*t*) reflect. For ∆*t*Lim,TJY = 4 s, both vSet and vTJY(*t*) cannot maintain the local maximum of vLim,Map,v˙ at *s* = 500 m, however.

Since the course of vSet is not jerk-free and must be smoothed by the trajectory, close proximity of vTJY(*t*) to vSet is not intended in all situations. The extent to which the trajectory can follow vSet is determined by the weighting factors *R* −1 v , *R* −1 a , *R* −1 j and the temporal distance of neighboring knots <sup>∆</sup>*t*κ . Furthermore, the temporal distance of neighboring data points ∆*t*It needs to be sufficiently small. Reducing ∆*t*It increases the computational effort of RBA linearly.

In the upper v−*s* diagram of Figure 5.2 vLim,Map,v˙ is identical for all trajectories. The lower diagram depicts the same quantities versus time. In a v − *t* diagram

Figure 5.2: Influence of temporal safety margin to upper speed limit ∆tLim,TJY on set point of trajectory velocity vSet and trajectory velocity vTJY. Only a subset vSet is shown. Upper diagram: The speed limit from map data with desired acceleration vLim,Map,v˙ is identical for both trajectories. Lower diagram: vLim,Map,v˙ differs with the trajectories. *Adopted from [79].*

vLim,Map,v˙ needs to be shown individually for each trajectory because when two trajectories differ, in general they have a different trajectory position at the same time *t*, hence vLim,Map,v˙ also differs between the two trajectories. *Adopted from [79]*. A v − *s* diagram is often more convenient for comparisons because of the identical vLim,Map,v˙ . However, the lower v − *t* diagram in Figure 5.2 illustrates well that the temporal safety margin to upper speed limit leads to a constant time gap between the courses of vSet and vLim,Map,v˙ .

Moreover, because of the constant temporal density of degrees of freedom resulting from the temporal distance of neighboring knots <sup>∆</sup>*t*κ , the curvature of the trajectory is independent of velocity in the v − *t* diagram. In contrast, in the

Figure 5.3: Iterative trajectory optimization using RBA: Speed limit from map data with desired acceleration vLim,Map,v˙ (black) and trajectory after 100 (green), 200 (yellow), 300 (red) and 400 (blue) iterations p.

v − *s* diagram the trajectory curvature usually decreases as velocity increases. Due to the equidistant knots with respect to time and (5.12), the trajectory can approximate the upper speed limit very close at low speeds because its degrees of freedom are dense with respect to position whereas at high speeds the approximation is less close.

#### **Advantage of iterative approach of RBA**

LM, RBA and NRBA compute the trajectory iteratively but in different ways. LM adapts all control points simultaneously, hence the whole trajectory is improved during an iteration. In contrast, RBA and NRBA process only a subset of the control points simultaneously and the temporal length of the trajectory increases with the number of iterations.

Figure 5.3 shows a vTJY(*t*) trajectory generated using RBA after 100, 200, 300 and 400 iterations with <sup>∆</sup>*t*κ <sup>=</sup> 2 s, <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.1 s and <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 2 s. The iterative approach of RBA allows to pause the calculation and use the intermediate result before continuing the calculations. This possibility is beneficial in presence of restrictions on computation time. It is also known that the optimization is finished for the subtrajectory that can be computed from knots and control points that are not used by RBA anymore but have been shifted out of the KF.

Furthermore, with RBA the number of data points is not required to be bounded because of the shifting function. This is beneficial because a priori it is not known how much time data points *t*<sup>p</sup> the trajectory needs in order to reach the end of the electronic horizon. Also the length of the electronic horizon usually increases as the navigation system is recalculating the route (c.f. Section 5.1). RBA can start trajectory optimization immediately parallel to the route recalculation and can add knots during the optimization process as needed.

In contrast, LM cannot change the knots. Hence, the number of needed knots has to be overestimated, which causes additional computational effort, because more control point values are computed.

#### **Influence of temporal distance of neighboring knots and number of spline intervals**

Figure 5.4 shows trajectories with <sup>∆</sup>*t*Lim,TJY <sup>=</sup> <sup>1</sup>.5 s and <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.2 s. Trajectories with temporal distance of neighboring knots <sup>∆</sup>*t*κ <sup>=</sup> 1 s are depicted in red and trajectories with <sup>∆</sup>*t*κ <sup>=</sup> 5 s in blue. For both cases trajectories computed by RBA with number of spline intervals *I* = 1 and *I* = 5 are compared to the LM solution. The solutions of both algorithms are similar and differences can mainly be identified from the acceleration diagram.

Increasing the temporal distance of neighboring knots <sup>∆</sup>*t*κ reduces the possible oscillation of velocity in a given time interval. This effect can be seen in the acceleration diagram for 200 m ≤ *s* ≤ 600 m. However, a large change of <sup>v</sup>TJY as for 600 m <sup>≤</sup> *<sup>s</sup>* <sup>≤</sup> 800 m can still be represented by <sup>∆</sup>*t*κ <sup>=</sup> 5 s through correspondingly larger differences of temporally less dense neighboring control point values. For <sup>∆</sup>*t*κ <sup>=</sup> 1 s, the RBA solutions are closer to the LM solution than for <sup>∆</sup>*t*κ <sup>=</sup> 5 s.

Figure 5.4: Influence of temporal distance of neighboring knots <sup>∆</sup>tκ on trajectory optimization using RBA and LM and influence of number of spline intervals I on trajectory optimization using RBA.

Increasing *I* also enables to smooth the trajectory because the filter can rework larger parts of the trajectory with hindsight when additional information is available. However, this can result in undesired velocity oscillations, e.g. for 800 m ≤ *s* ≤ 1200 s. This applies to both RBA and LM.

Furthermore, RBA trajectories with larger *I* are more likely to exceed vLim,Map,v˙ . This slightly occurs at *<sup>s</sup>* <sup>=</sup> 1200 m where the RBA trajectory with <sup>∆</sup>*t*κ <sup>=</sup> 5 s accelerates too early. The reason is a general flaw of the filter-based approach. When the filter adapts control points of previous spline intervals according to the knowledge of its state estimation covariance matrix, it does not compute any deviations or errors for these control points that could be penalized. Exceeding vLim,Map,v˙ can be avoided by increasing ∆*t*Lim,TJY with *I*.

As LM evaluates the whole sum of squared errors in the approximated NWLS problem in each iteration, this problem does not occur with LM. At around *s* = 1200 m both LM trajectories are identical.

Although the above stated approach does not optimize the energy consumption by taking into account the electrical power explicitly, a reduction of energy consumption can be achieved implicitly by increasing the weight of acceleration error square *R* −1 a . The method of reducing acceleration and braking was also chosen in [1, 107, 112]. According to the investigations in [81] it is less effective than using an energy consumption model. Instead its advantage is that a simpler optimization algorithm can be used.

# **5.4 Trajectory optimization considering electrical power**

 According to Section 4.4 low absolute values of the electrical traction power *P*trac,elec lead to low power losses. *P*trac,elec is described by the adaptive electrical power model (AEPM) proposed in Subsection 4.7.2. The model output *P*AEPM is given by

$$P\_{\rm AEPM}(t) = \text{AEPM}(\nu\_{\rm TIV}(t), a\_{\rm x}).\tag{5.13}$$

The sensor longitudinal acceleration *a*<sup>x</sup> from (4.22) is computed as

$$a\_{\rm x} = a\_{\rm TOY}(t) + \mathbf{g} \cdot \sin(\alpha(s\_{\rm TOY}(t))) \tag{5.14}$$

and the road slope angle α can be derived from the road slope stated in percent according to map data using (4.2).

When considering the required electrical power in trajectory planning, (5.10) is augmented with the summand *R* −1 P · (*P*AEPM) <sup>2</sup> which penalizes absolute values of *P*AEPM. *R* −1 P denotes the weight of power error square and its reciprocal *R*<sup>P</sup> is the variance of power measurement.

The resulting optimization problem reads

$$\begin{split} \hat{\mathbf{x}} = \underset{\mathbf{x}}{\arg\min} & \sum\_{p=1}^{P} \left( R\_{\upsilon}^{-1} \cdot \left[ \nu\_{\text{Lim}, \text{TOY}} \left( s\_{\text{TOY}}(t\_{p}) \right) - \nu\_{\text{TOY}}(t\_{p}) \right]^2 \right. \\ & \left. + R\_{\alpha}^{-1} \cdot \left( a\_{\text{des}} - a\_{\text{TOY}}(t\_{p}) \right)^2 + R\_{j}^{-1} \cdot \left( j\_{\text{TOY}}(t\_{p}) \right)^2 \right. \\ & \left. + R\_{P}^{-1} \cdot \left( P\_{\text{AEPM}}(t\_{p}) \right)^2 \right). \end{split} (5.15)$$

As *P*AEPM correlates with the product of vTJY and its derivative *a*TJY, an approximation of this fourth optimization goal such that RBA can be applied, seems not promising for good results. *Adopted from [79]*.

Most nonlinear filter algorithms such as UKF or PF do not accept a measurement matrix that relates a control point vector to a measurement as input. Instead, they apply a given nonlinear measurement function to a set of control point vectors and use the resulting function values for estimating the relation between measurement and control points. However, for the first three optimization goals the exact values of the measurement matrix are known. These result from the values of the B-spline basis functions.

Estimating the B-spline basis functions and additionally performing the transition from a spatial to a temporal representation poses a challenge for the convergence of a nonlinear filter. Experiments were conducted with a squareroot cubature Kalman filter (SCKF) stated in [6, 7]. The SCKF can be seen as a special case of the UKF. In experiments no weighting factor combination was found that yielded consistent convergence of the trajectory to the upper speed limit. This was also the case when the SCKF was applied to optimization problem (5.10), which does not consider electrical power.

The MPF is beneficial for problems that can be subdivided in a linear subproblem and a nonlinear subproblem. A KF solves the linear subproblem optimally using a given measurement matrix while a PF is applied to the nonlinear problem [155]. In the trajectory optimization application the nonlinear problem is the fourth optimization goal. The MPF converges for problem (5.15) reliably

provided that the weighting factors are within a certain range. For these reasons, NRBA includes a MPF. However, even though the MPF comprises a KF, the convergence radius of the MPF is still noticably smaller than that of a pure KF and the MPF is much more sensitive to filter parameter variations.

 Braking for an upcoming curve usually requires negative *P*AEPM for recuperation. Strong penalization of *P* 2 AEPM can prevent braking for upcoming curves. In order to enable sufficient braking power, the computation of the error *e* = vLim,TJY − vTJY within the MPF is designed asymmetrically. If *e* ≤ 0, the artificial measurement <sup>v</sup>Lim,TJY is replaced with <sup>v</sup>Lim,TJY <sup>=</sup> *<sup>R</sup>*v>vLim · *<sup>e</sup>* <sup>+</sup> <sup>v</sup>TJY. Hence, *<sup>e</sup>* is multiplied with *<sup>R</sup>*v>vLim if <sup>v</sup>TJY is above <sup>v</sup>Lim,TJY. In case of *<sup>e</sup>* <sup>&</sup>gt; <sup>0</sup> the original *<sup>e</sup>* is multiplied with *<sup>R</sup>*v<vLim, the error weighting if <sup>v</sup>TJY is below vLim,TJY. In each case the resulting error square is afterwards weighted with *R* −1 v in the MPF. *Adopted from [79]*.

*<sup>R</sup>*v>vLim cannot be chosen arbitrarily large. First, *<sup>R</sup>*v>vLim <sup>&</sup>gt; <sup>2</sup> can prevent the MPF from converging to the upper speed limit. Second, during operation of the ALC situations can occur in which the vehicle exceeds vLim,Map,v˙ . An example is that the driver intervens and accelerates in order to overtake a slower vehicle. At its beginning the trajectory needs to indicate the current vehicle velocity. *<sup>R</sup>*v>vLim should be only so large that a clearly noticable braking occurs until the vehicle is below vLim,Map,v˙ . For *<sup>R</sup>*v>vLim <sup>&</sup>gt; <sup>2</sup> braking according to the computed trajectory is uncomfortably abrupt.

 The asymmetrical weighting of the velocity error can also be used in combination with RBA but since there is no incentive to exceed vLim,Map,v˙ because of the missing optimization goal regarding *P*AEPM, the asymmetric weighting has very limited influence. *Adopted from [79]*.

#### **Influence of weight of power error square**

 By increasing the weight of power error square *R* −1 P or by lowering the variance of power measurement *R*P, respectively, the trajectory can be optimized towards low energy consumption. Figure 5.5 depicts various quantities for

Figure 5.5: Relevant quantities of trajectories determined by NRBA and LM for different variance of power measurement. Velocity v, electrical traction power Ptrac,elec, road slope <sup>γ</sup>, energy loss between HV battery and wheel <sup>E</sup>Loss and HV battery energy <sup>E</sup>Batt for trajectories determined by NRBA and LM that differ in the variance of power measurement RP. *Adopted from [79].*

trajectories determined by NRBA with *R*<sup>P</sup> = 10<sup>2</sup> , *R*<sup>P</sup> = 10<sup>3</sup> and *R*<sup>P</sup> = 10<sup>4</sup> . For comparison, the LM solution for *R*<sup>P</sup> = 10<sup>4</sup> is also shown. The remaining trajectory parameters are <sup>∆</sup>*t*κ <sup>=</sup> 2 s, <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.25 s, <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s and *<sup>I</sup>* <sup>=</sup> <sup>1</sup>.

In the depicted situation, all tractories indicate roughly the same velocity at the start position and also at the end position. Hence, the kinetic and potential energy of a vehicle at the beginning and end is roughly the same regardless of which trajectory it tracks. Therefore the energy consumptions resulting from following each of the trajectories can be compared.

For *R*<sup>P</sup> = 10<sup>4</sup> the depicted quantities for the NRBA and LM trajectories are very similar and both trajectories follow vLim,Map,v˙ closely. With LM the energy loss between HV battery and wheel (*E*Loss) is *E*Loss = 77 Wh and the required HV battery energy (*E*Batt) is *E*Batt = 721 Wh. For *R*<sup>P</sup> = 10<sup>4</sup> NRBA achieves *E*Loss = 75 Wh and *E*Batt = 714 Wh.

When *R*<sup>P</sup> is lowered, peaks in the electrical traction power *P*trac,elec are reduced which translates to less energy loss between HV battery and wheel as well as less HV battery energy. *R*<sup>P</sup> = 10<sup>3</sup> leads to *E*Loss = 71 Wh and *E*Batt = 703 Wh. For *R*<sup>P</sup> = 10<sup>2</sup> *E*Loss = 65 Wh and *E*Batt = 684 Wh result.

Regarding the driving style it can be observed that with lower *R*P, the trajectory exhibits lower acceleration, stays more below vLim,Map,v˙ , avoids short-lasting velocity peaks and tends to oscillate more. *Adopted from [79]*. The oscillations can mainly be explained by the larger slope variations which affect *P*trac,elec but they partly also result from the fact that the MPF estimations are more strongly based on the state-space sampling of the PF than on the KF. With LM no energy savings were achieved by lowering *R*P, even after varying LM parameters. Instead, the trajectory diverged which indicates a narrow convergence radius of the unapproximated nonlinear optimization problem.

 During the acceleration phase up to *s* = 1000 m the NRBA does clearly avoid peaks in *P*trac,elec by reducing the acceleration at the slope. However, this does apply to the deceleration phase not to such an extent, even under consideration that from *s* = 2200 m on there is a high road grade which avoids the need for large negative *P*trac,elec. The filter could still smooth power demand more by decelerating earlier. It was also oberserved during road sections with negligible slope that NRBA decelerates too late and therefore the asymmetrical weighting of the velocity error was added. *Adopted from [79]*.

As shown in Subsection 3.3.4 and Subsection 3.4.4, both novel filter based methods RBA and NRBA can reproduce a symmetrical approximation of WLS and LM, respectively. Provided that *I* is large enough, the filter lag is negligible with RBA. When the nonlinear optimization goal is weighted heavily with NRBA, a filter lag occurs, solutions become more oscillating and increasing *I* is not as unambigiously beneficial as with RBA. In the situation depicted by Figure 5.5, increasing *I* mainly leads to more oscillating trajectories whereas no significant change in deceleration can be noticed. Hence, the energy-saving potential of NRBA during recuperation is lower than expected.

Increasing *I* should facilitate early deceleration because NRBA can adapt larger parts of the trajectory with hindsight when data that involves the lower speed limit is processed. MPF improvements that achieve the same approximation quality with less particles as stated in [203] would be beneficial for good results with larger *I*. They could help to overcome the flaw concerning deceleration because they enable to keep the state-space sampling density in each dimension constant when *I* is increased.

At an early stage, at which the trajectory representation was not yet temporal, the AEPM hyperparameters were not final and NRBA still included the SCKF instead of the MPF, automated tests for finding suitable target criteria weightings were done by A. Thorgeirsson [173] as a student assistant.

# **5.5 Considering trajectory constraints**

After each iteration of the trajectory optimization, a lower limit on the trajectory velocity vTJY is enforced and the trajectory acceleration *a*TJY is restricted. This ensures that after any iteration a feasible trajectory is available. Due to the high execution frequency the formulae are rather simple, avoid evaluating the B-spline function and take advantage of the convex hull property.

vTJY is restricted to nonnegative values, which translates to restricting all components of the state estimate xˆ to nonnegative values:

$$
\hat{\mathfrak{x}} = \min \left( \hat{\mathfrak{x}}, \mathbf{0} \right) \tag{5.16}
$$

Additionally the differences between neighboring components *x*ˆ of xˆ are limited according to

$$\begin{aligned} \text{f}\,\hat{\mathbf{x}}\_{i} &= \max\left(\hat{\mathbf{x}}\_{i-1} + a\_{\text{des,neg}} \cdot \Delta t\_{\kappa}, \min\left(\hat{\mathbf{x}}\_{i}, \hat{\mathbf{x}}\_{i-1} + a\_{\text{des,pos}} \cdot \Delta t\_{\kappa}\right)\right), \\\ \hat{\mathbf{z}} &= \mathbf{2}, \mathbf{3}, \dots \end{aligned} \tag{5.17}$$

*a*des,pos denotes the desired positive longitudinal acceleration and *a*des,neg is the desired negative longitudinal acceleration. The approach in (5.17) overestimates the absolute value of trajectory acceleration using the convex hull property of the B-spline function. Hence, (5.17) might in some situations adjust control points although the specified acceleration limits are not yet exceeded.

The following subsections state further constraints which are taken into account partly as *hard constraints* and partly as *soft constraints*. Hard constraints are constraints that reduce the solution space of an optimization problem and can lead to an empty set of feasible solutions. In contrast, the term soft constraints means that the optimization function is chosen such that violating these soft constraints leads to very bad function values with the aim that such solutions are avoided by the optimization method.

### **5.5.1 Adaption to vehicle motion state**

 Small deviations between trajectory velocity vTJY and vehicle velocity vVhcl always occur because of an imperfect control. However, there are several cases in which both quantities differ significantly from each other. Examples are driving off from a parking position and when the driver stops overriding the ALC by pressing accelerator or braking pedal.

If the deviation exceeds a tolerated threshold, a new trajectory should be planned. The velocity and acceleration at the trajectory beginning should be adapted to the vehicle motion state given by vehicle velocity vVhcl and vehicle longitudinal acceleration v˙Vhcl,x. Hence, the constraints

$$\begin{aligned} \nu\_{\rm TJY}(t\_1 = 0) &= \nu\_{\rm Vhcl} \\ a\_{\rm TJY}(t\_1 = 0) &= \dot{\nu}\_{\rm Vhcl,x} \end{aligned} \tag{5.18}$$

shall be enforced. Several methods for taking into account state constraints during Kalman filtering are presented in [163]. The state projection method can modify the control points such that the trajectory fulfills certain hard equality constraints at single points in time. *Adopted from [79]*. The state projection method projects an unconstrained estimate xˆ + <sup>p</sup> onto the hyperplane defined by the constraints Dx = d. Usually Dx = d is an underdetermined equation system. The constrained estimate x˜ + p is given by

$$\hat{\mathfrak{x}}\_p^+ = \hat{\mathfrak{x}}\_p^+ - \mathbf{D}^\top \left(\mathbf{D} \mathbf{D}^\top\right)^{-1} \left(\mathbf{D} \hat{\mathfrak{x}}\_p^+ - \mathbf{d}\right). \tag{5.19}$$

The difference of the projected control points need to be checked against the desired longitudinal acceleration. If they exceed the acceleration limits *a*des,neg or *a*des,pos, the projection needs to be repeated with additional constraints that refer to differences between neighboring control point values.

 The state projection method changes only the control points that influence the trajectory function at *t*<sup>1</sup> = 0. If the function is projected onto a vehicle at standstill and the temporal distance of neighboring knots <sup>∆</sup>*t*κ between knots is small, the trajectory will demand an abrupt acceleration towards the upper speed limit unless control points that refer to following spline intervals are adapted too.

A jerk-free and comfortable velocity transition can be achieved by modifying in the optimization problems (5.10) and (5.15) the desired acceleration value

Figure 5.6: Individual effects of projection onto vehicle motion state using (5.19) (1), enforcement of acceleration constraints with (5.17) (2) and adaption of filter settings using (5.20) (3) on the trajectory during trajectory adaption to vehicle motion state.

*a*Set(*t*p) from (5.11) and the acceleration weight *R* −1 a in the first filter iterations as follows:

$$R\_a^{-1} = R\_{a, t\_0}^{-1} + (R\_a^{-1} - R\_{a, t\_0}^{-1}) / \Delta t\_{\text{VhelAdapt}} \cdot \min(\Delta t\_{\text{VhelAdapt}}, t),$$

$$a\_{\text{Set}}(t\_p) = \dot{\nu}\_{\text{Vhel}, \text{x}} + (0 - \dot{\nu}\_{\text{Vhel}, \text{x}}) / \Delta t\_{\text{VhelAdapt}} \cdot \min(\Delta t\_{\text{VhelAdapt}}, t). \tag{5.20}$$

∆*t*VhclAdapt and *R* −1 a,t0 are tuning parameters. From an initially strong weighting *R* −1 a,t0 of *a*Set = v˙Vhcl,x at *t* = 0, the quantities *R* −1 a and *a*Set return to their standard values in a linear fashion until *t*<sup>0</sup> + ∆*t*VhclAdapt. *Adopted from [79]*.

Figure 5.6 illustrates the individual effects of projection onto vehicle motion state with (5.19), enforcement of acceleration constraints using (5.17) and adaption of filter settings according to (5.20) on the trajectory. The trajectory parameters are <sup>∆</sup>*t*κ <sup>=</sup> 2 s, <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.1 s, <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 2 s and *<sup>I</sup>* <sup>=</sup> <sup>1</sup>.

In the depicted situation, the speed limit from map data with desired acceleration vLim,Map,v˙ is constant at <sup>26</sup>.7 m/<sup>s</sup> (black solid line in upper diagram). The vehicle motion state consists of the vehicle velocity <sup>v</sup>Vhcl <sup>=</sup> 20 m/<sup>s</sup> and the vehicle longitudinal acceleration <sup>v</sup>˙Vhcl,x <sup>=</sup> <sup>−</sup>1 m/<sup>s</sup> 2 . Each component is indicated by a black dot in the corresponding diagram. A trajectory without adaption (depicted by green dotted line) starts at vLim,Map,v˙ instead of vVhcl. A trajectory that is only projected onto the vehicle state (orange dashed line) can demand strong accelerations since the projection only changes the first four control points. The red solid line results from additionally enforcing acceleration constraints with (5.17). The adaption of the filter setting using (5.20) with *R* −1 a,t0 <sup>=</sup> <sup>0</sup>.<sup>01</sup> and ∆*t*VhclAdapt = 150 s allows to get a more comfortable transition.

The desired effect could also be achieved using a sigmoid function. However, the parameters of the sigmoid function need to be chosen carefully because filter sensitivity differs strongly with the magnitude of *R* −1 a . For simplicity a linear transition with rather high ∆*t*VhclAdapt was used.

### **5.5.2 Adaption to previous trajectory**

vTJY(*t*1), *a*TJY(*t*1) and *j*TJY(*t*1) with *t*<sup>1</sup> = 0 of a new trajectory can also be projected onto vTJY, *a*TJY and *j*TJY of the current trajectory at its current evaluation point *t* ∗ in order to achieve a *C* 2 continuous connection between both trajectories.

When connecting two trajectories using state projection, the knot positions of the new trajectory are additionally chosen such that the distance of *t*<sup>1</sup> to the neighboring knots equals the distances of *t* ∗ to the neighboring knots of the previous trajectory. Then the shape of the new trajectory around *t*<sup>1</sup> is identical to that of the previous trajectory around *t* ∗ and uncomfortable velocity oscillations at the joint are avoided.

)

Figure 5.7: Individual effects of projection (1) and improved control point initialization (2) on the new trajectory during adaption to current trajectory at evaluation point sTJY(t ∗ ) of current trajectory.

Figure 5.7 depicts various trajectories with <sup>∆</sup>*t*κ <sup>=</sup> 1 s, <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s, <sup>∆</sup>*t*It <sup>=</sup> <sup>0</sup>.1 s and *<sup>I</sup>* <sup>=</sup> <sup>3</sup>. At *<sup>t</sup>* <sup>∗</sup> <sup>=</sup> <sup>14</sup>.333 s or *<sup>s</sup>*TJY(*<sup>t</sup>* ∗ ) <sup>=</sup> <sup>201</sup>.75 m, respectively, replanning occurs and the new trajectory needs to be connected to the current trajectory depicted in green. The new trajectory depicted in orange is not projected and therefore the connection is not continuous. The new trajectory depicted in red is projected and a *C* 2 continuous connection to the current trajectory is achieved. However, shortly behind the connection point the courses of its derivatives deviate from the corresponding courses of the current trajectory. The blue trajectory results when additionally the control point vector of the new trajectory is initialized with the corresponding control point values of the current trajectory. The deviations in the courses of the derivatives are significantly reduced.

However, in case of *I* = 1 the second derivative still deviates with this approach. This is because the estimate cannot be improved with hindsight using the knowledge stored in the KF covariance matrix that results from additional data points.

#### **5.5.3 Adaption to vehicle ahead**

The Intelligent Driver Model (IDM) described in [174] provides an ACC functionality by computing a desired vehicle acceleration *a*IDM given by

$$a\_{\rm IDM} = a\_{\rm free} + a\_{\rm int}.\tag{5.21}$$

The free-road acceleration term *a*free with

$$a\_{\rm free} = a\_{\rm des,pos} \left[ 1 - \left( \frac{\nu\_{\rm Vhcl}}{\nu\_{\rm Set}} \right)^{\delta} \right] \tag{5.22}$$

determines how the vehicle velocity vVhcl converges to the set velocity vSet without a vehicle ahead. *a*des,pos is the desired positive longitudinal acceleration. The acceleration exponent <sup>δ</sup> is usually <sup>4</sup>. The interaction acceleration term *<sup>a</sup>*int with

$$a\_{\rm int} = a\_{\rm des,neg} \left(\frac{\Delta s\_{\rm des}}{\Delta s\_{\rm VhclAlhead}}\right)^2 \tag{5.23}$$

causes the vehicle to decelerate in order to adjust the distance to the vehicle ahead ∆*s*VhclAhead to the desired minimum distance ∆*s*des given by

$$
\Delta \mathbf{s}\_{\rm des} = \Delta \mathbf{s}\_{\rm Jam} + \Delta t\_{\rm Vhcl} \mathbf{A}\_{\rm head} \cdot \boldsymbol{\nu}\_{\rm Vhcl} + \frac{\nu\_{\rm Vhcl} \cdot (\boldsymbol{\nu}\_{\rm Vhcl} - \boldsymbol{\nu}\_{\rm Vhcl} \mathbf{A}\_{\rm head})}{2\sqrt{a\_{\rm des,pos} \cdot |a\_{\rm des,neg}|}}. \tag{5.24}$$

∆*s*VhclAhead and the velocity of the vehicle ahead vVhclAhead are measurements of the radar sensor. ∆*s*Jam denotes the desired traffic jam distance with a typical value of 2 m. ∆*t*VhclAhead is the desired time gap to the vehicle ahead and *<sup>a</sup>*des,neg <sup>&</sup>lt; <sup>0</sup> the desired negative longitudinal acceleration.

vSet in (5.11) can be modified with *a*IDM during the iterative solution of (5.10) and (5.15) such that the trajectory optimization process takes into account the vehicle ahead as a soft constraint. The radar sensor data ∆*s*VhclAhead and vVhclAhead refers to the new trajectory at start time *t*<sup>1</sup> = 0. Under the assumption that vVhclAhead is constant and that the ego vehicle will follow the planned trajectory perfectly (vVhcl(*t*) <sup>=</sup> <sup>v</sup>TJY(*t*), <sup>∀</sup>*<sup>t</sup>* <sup>≥</sup> <sup>0</sup>), <sup>∆</sup>*s*VhclAhead can be calculated in following iterations as the sum of the initial distance ∆*s*VhclAhead(*t*1) and vVhclAhead(*t*1) · *t*, from which the trajectory position *s*TJY is subtracted:

$$
\Delta \text{sV}\_{\text{hcl}} \text{Ahead}(t\_P) = \Delta \text{sV}\_{\text{hcl}} \text{Ahead}(t\_1) + \nu\_{\text{Vhcl}} \text{Ahead}(t\_1) \cdot t\_P - \text{sT}\_{\text{T} \text{Y}}(t\_P) \tag{5.25}
$$

In order to avoid dividing by small values, the IDM interaction free road term from (5.22) with velocity dependent *a*des,pos according to the acceleration tables and with <sup>v</sup>Set <sup>=</sup> <sup>v</sup>TJY is used only if <sup>v</sup>TJY is larger than 2 <sup>m</sup>/s. For <sup>v</sup>TJY <sup>≤</sup> 2 m/s, *a*free is set to the trajectory acceleration *a*TJY. The interaction term remains as in (5.23).

<sup>v</sup>Set is determined depending on a hysteresis with respect to *<sup>a</sup>*int: If *<sup>a</sup>*int <sup>&</sup>gt; <sup>−</sup>0.05 m/<sup>s</sup> 2 , vSet for trajectory optimization is computed according to (5.11). If

Figure 5.8: Effect of Intelligent Driver Model (IDM) on set point of trajectory velocity vSet and resulting trajectory depending on the chosen desired time gap to the vehicle ahead ∆tVhclAhead during trajectory adaption to a vehicle ahead with a velocity of 12 m/s.

*<sup>a</sup>*int <sup>&</sup>lt; <sup>−</sup>0.15 m/<sup>s</sup> 2 , braking because of a vehicle ahead is required and *a*IDM is used to correct vSet downwards:

$$\nu\_{\text{Set}} = \max\left(0, \min\left(\nu\_{\text{TJY}} + \Delta t\_{\text{It}} \cdot a\_{\text{IDM}}, \nu\_{\text{Lin,TIY}}\right)\right) \tag{5.26}$$

In experiments it turned out beneficial to let *a*Set remain at zero. With these modifications the free-road term in (5.22) causes a comfortable acceleration or deceleration towards the current vTJY coming from the upper speed limit.

The minimum operation in (5.26) ensures that vTJY can decrease fast enough. Without this minimum operation, the IDM can weaken the braking for a tight upcoming curve when there is no vehicle ahead. This is because (5.22) causes a deceleration towards a lower velocity set point vSet that is sufficient and comfortable on highways but too weak for country roads, on which vSet can vary strongly and deviations between vVhcl and vSet should be minimal.

Figure 5.8 shows a situation in which the IDM adapts the trajectory using (5.26). If the vehicle trajectory tracks the planned trajectory and the velocity of the vehicle ahead remains constant, the specified desired time gap to the vehicle ahead will result.

## **5.6 Scientific contribution**

DM are popular for automotive applications and have great potential in combination with DP as their orthogonal features can complement each other. However, the exponential growth of computational effort with increasing time horizon limits the application of DM to short time horizons.

In general the resulting static optimization problem (2.7) in the DM approach is nonlinear and solved by sequential quadratic programming (SQP) techniques or interior point methods [187]. Frequently trajectory optimization is applied to vehicles with combustion engine or a hybrid power train, both of which have

Figure 5.9: Classes and examples of optimization problems

lots of degrees of freedom and constraints. For example, if the optimization problem incorporates the gear selection it is a mixed integer problem.

In comparison, BEVs often have a power train with 1-speed gear box which simplifies the optimization problem. Quadratic problems are a subset of nonlinear problems that can be optimized more efficiently with QP methods because setting the derivative of the convex optimization function to zero results in sufficient conditions for the global optimum.

The common linear WLS approximation problem is an unconstrained quadratic optimization problem and the NWLS approximation an unconstrained nonlinear optimization problem. Figure 5.9 depicts different classes of optimization problems.

This work presents a DM based trajectory optimization approach for an ALC of a BEV with fixed gear ratio such as the considered research vehicle. With the presented approach, the computational effort only grows linearly with the number of function parameters or the time horizon. This substantial saving is achieved by formulating the trajectory optimization problem as either a WLS approximation problem or a NWLS approximation problem. The approximation problem is solved iteratively by RBA or NRBA, respectively. Each iterative method includes an iterative state estimator, also known as *filter*.

Usually filters solve unconstrained linear and nonlinear WLS problems. Therefore limitations of the vehicle and restrictions of the environment enter the trajectory optimization problem in the presented approach mainly as soft constraints. This means that the objective function is designed such that undesired solutions coincide with comparatively very bad values of the objective function. However, some hard constraints are also enforced using the state projection method from [163]. Hard constraints narrow the solution space of the optimization problem.

The trajectory is defined by a B-spline function and describes the desired vehicle velocity with respect to time. The trajectory results from an iterative solution of the approximation problem. Spatially or temporally defined soft constraints or optimization goals referring to derivatives of the function or its integral can be taken into account during the optimization process. For example, a spatial upper limit on the velocity defined by the trajectory results from the legal speed limit or tight curves.

Treating such spatial constraints as soft constraints leads to a NWLS problem. However, the iterative trajectory optimization approach allows to approximate this problem in each time step by a WLS problem similar to [28, 47], who approximated a nonlinear problem by a quadratic problem in order to be able to apply a QP method instead of a SQP method. The linear state estimator for WLS problems enables large savings in computational effort compared to the nonlinear state estimator for NWLS problems.

The energetic optimization of the trajectory is a case in which the approximation with a WLS problem cannot be expected to produce acceptable results. Then a nonlinear state estimator can be applied as demonstrated.

The presented iterative trajectory optimization approach offers the following additional advantages:

First, the number of function parameters does not need to be bounded. Instead, the iterative estimator can determine additional function parameters if the temporal length of the trajectory needs to be increased. As this does not influence the computational effort in each iteration of the estimator, arbitrarily long trajectories can be planned.

Second, the temporal length of the trajectory increases with the iterations and after each iteration an intermediate result is available for use. Therefore, the optimization can be paused and continued in accordance with the time constraints.

#### **Differentation from previous works**

The previous project *e-generation* [22, pp. 29-35] uses a spatially defined velocity trajectory represented by a cubic polynomial. The polynomial is iteratively adapted to map data using a KF in such a way that an optimization problem similar to (5.3) but with additional target criteria referring to the first two derivatives is solved.

Due to few degrees of freedom of the cubic polynomial, only a trajectory with simple shape can be represented. The spatially defined velocity trajectory exhibits the flaws mentioned in Section 5.2. Especially the reluctant behavior at low velocities is noticable in test drives. A target criterion referring to the electrical traction power or the consumed energy is not included in the optimization problem.

The approach presented in this chapter ensures nonnegative trajectory velocities at the spline function level by restricting its control point values via (5.16). In contrast, the nonnegativity of the polynomial is not ensured, only the result of the polynomial evaluation is restricted to nonnegative values.

Furthermore, the previous approach adapts the trajectory only roughly to the vehicle velocity and acceleration by modification of the target values in the approximation, whereas in (5.18) an exact adaption via parameter projection is done.

An adaption to a previous trajectory as in Subsection 5.5.2 is not needed with the polynomial function and constraints resulting from a vehicle ahead as in Subsection 5.5.3 were not considered.

# **6 Automated energy-efficient longitudinal control**

Section 6.1 of this chapter states the stages of the development process of the ALC followed by a description of the architecture of the ALC and its components in Section 6.2. Section 6.3 investigates the influence of selected parameters on the ALC energy-saving potential in simulations. Section 6.4 summarizes the technical contribution of this work concerning ALC.

## **6.1 Development process**

The development of the ALC in the project *e-volution* followed the V model, which describes the software development process in the shape of the letter V and is depicted in Figure 6.1. The V model is widely used in the automotive industry [152, p. 25]. ISO 26262 states requirements for the development process of safety critical components and systems in vehicles. The procedure described in ISO 26262 is oriented towards the V model [187, pp. 110-111].

In the descending branch of the V model the customer requirements are analyzed and translated into a logical architecture from which a technical architecture is derived. In subsequent steps this technical architecture is decomposed into systems and components. During this process on each level specifications and test cases are defined for later review of the development steps. The last step of the descending branch coincides with the first step of the ascending branch and includes the implementation of the specified components.

Figure 6.1: Software development process according to V model with allocation of in-the-Loop methods according to [187, p. 165].

The ascending branch comprises subsequent testing and integration steps from individual components up to testing the customer acceptance on the whole system.

Each step of the descending branch defines specifications that have to be met during verification on the same level of the ascending branch as indicated by dashed arrows in Figure 6.1 [187, pp. 163-168].

The V model distinguishes four different test steps. The first three are component test, integration test and system test. These are verificiation tests. Verification denotes the process of evaluating whether an implementation meets the specified requirements for the corresponding development phase. Only the customer acceptance test is a validation test that determines whether all customer requirements are fulfilled. Methodological additions to the V model such as rapid prototyping enable validation at an early stage and help to avoid timeconsuming and costly reworking loops across various stages of the V model [152, pp. 33, 152-154].

Rapid prototyping denotes process steps that enable early verification or validation of specifications. Typical process steps are modelling, simulation, integration and test of the prototype in the vehicle [152, pp. 160-162]. Model-based methods can be applied for specification of software functions as well as validation of specifications. Model-based software development allows to model software functions in a graphic environment using block diagrams [152, pp. 31- 32]. A model compiler can compile the software functions for various target hardware such that the functions can be simulated as well as tested in the vehicle [152, p. 208]. If rapid prototyping methods support the development process, a virtual integration of the system is possible at the end of the descending branch. Before the integration steps this virtual prototype can be evalutated in simulated test drives [187, pp. 163-168].

In-the-Loop methods allow to combine models or real components at each development step with a reproduction of their real environment in order to obtain an assessable system. As there are no real components until the implementation phase of the V model, a simulation environment is used for virtual integration. Figure 6.1 allocates different in-the-Loop methods to the steps of the development process.

Model-in-the-Loop (MiL) enables a confirmation of the specification of customer requirements up to the logical architecture. The created model-based algorithms do not yet refer to the hardware of the target system. By transferring the model-based algorithms into a simulation environment that is hardware independent but exhibits already technical characteristics similar to that of the target system one can perform Software-in-the-Loop (SiL) which allows for an assurance up to individual components. Hardware-in-the-Loop (HiL) denotes methods that transfer models from the SiL environment to real components. For example a function can be executed on a vehicle ECU while the system architecture of the vehicle is simulated. This allows to verify the interaction of the ECU with other simulated vehicle components [187, pp. 163-168]. HiL simulation usually requires real-time capable components [152, pp. 297-301]. When HiL methods are applied consequently, the entire system exists in real

components and can be verified up to the logical architecture. The Vehiclein-the-Loop (ViL) method is especially useful for the development of driver assistance systems as it allows the operation of a real test vehicle in a virtual environment. Vehicle and virtual environment can be linked by either replacing the real sensors with interfaces to the virtual environment or by stimulating real sensors artificially.

In the research project *e-volution*, the customer requirement was stated as an ALC that is as comfortable and reliable as its predecessor from the previous project and offers at least the same dynamic driving style as the ALC that was developed during the previous project *e-generation* for the same research vehicle. As main enhancements the *e-volution* ALC was required to be capable of planning of longer, more far-sighted trajectories and of realizing an energyefficient driving style.

The specifications of logical and technical architecture were taken over from *e-generation*. The logical architecture can be summarized as a system that controls motor torque according to map data. The technical architecture defines the interfaces of the ALC to the vehicle, for example which CAN messages are sent and received. Furthermore, it specifies the interfaces to the driver, e.g. how the driver can control the ALC using buttons next to the steering wheel and which information about the ALC state is displayed by the instrument cluster.

System design specifies the software architecture that implements the ALC functionality. The functionality is divided into different components whose interfaces are defined. System design was conducted during *e-generation* and reused for *e-volution*. The system design is specified by a framework in the model-based development environment MATLAB Simulink by Math-Works. The framework was created by an employee of the Porsche Engineering Services GmbH. It also takes care of communication with other vehicle components via several CAN busses and converts CAN messages into physical quantities. Furthermore, it checks the torque demand of the ALC system output for plausibility. This allows a safe vehicle operation in conjunction with an agile development of the ALC. *Adopted from [79]*.

Design of components converts the black-box description of the system design into a white-box description for each module including the internal data flow such that each module can be implemented [187, pp. 168-173]. Within *e-volution*, component design led to the approximation algorithms RBA and NRBA and the AEPM. These components were implemented and tested in MATLAB. Then the trajectory planning based on RBA and NRBA was developed and tested in MATLAB using the open-loop vehicle reference model mentioned in Section 4.5 to investigate its energy-saving capabilities.

For further integration steps these functions were included into the Simulink framework. Translating MATLAB functions into sequences of Simulink blocks seemed not be advantageous. Therefore these functionalities were integrated into the Simulink framework as embedded MATLAB functions. As a result, each of the ALC components is based on few Simulink blocks that contain embedded MATLAB functions. Around the framework several simulink blocks including the closed-loop vehicle model from Section 4.5 were added in order to enable SiL tests of individual components and the whole ALC system. Extensions for considering constraints stated in Section 5.5, such as the projection of the trajectory as well as taking into account the vehicle ahead, were almost exclusively developed within the Simulink framework because of the closed control loop.

Additionally, a HiL test bench was used for component, system and integration tests as well as calibration. The HiL test bench was created by B. Fath during *e-generation* [22, pp. 60-73]. During HiL tests the developed functions run on the target hardware of the research vehicle. The target hardware is an ETAS ES910 rapid prototyping ECU and can execute compiled Simulink models. The HiL test bench allows to test the interaction of the software on ECU with the simulated vehicle architecture close to reality before the actual test in the vehicle.

The HiL test bench consists of four hardware components. A real-time computer, a host computer, the prototyping ECU and a car PC. Figure 6.2 shows

Figure 6.2: Simplified illustration of HiL architecture according to [22, p. 66]

the connections between the components of the HiL test bench in a simplified manner. The real-time computer is an IPG Xpack4 that runs a simulation environment including all CAN communication between the ES910 and the architecture of the virtual vehicle. The host PC is an ordinary desktop computer that controls the real-time computer via an ethernet connection, starts simulations, records simulation data and provides a graphic simulation output. The simulation software is IPG CarMaker HiL, a vehicle dynamics simulation software that is extended by the capability to control the Xpack4.

In CarMaker a vehicle model was created that is more detailed than the vehicle reference models described in Section 4.5 and also takes into account tire characteristics and vehicle kinematics. Furthermore, a model of the power train as described in Section 4.3 was integrated into the CarMaker vehicle model as a Simulink model and a road model of the WR was created from Open Street Map data and included in CarMaker. These models were also added by B. Fath.

The host PC also runs the experiment environment ETAS INCA. Via a second ethernet connection of the host PC, INCA can flash compiled Simulink models onto the ES910, start them on the ES910 and view and change model parameters during run-time, e.g. for calibration purposes.

The car PC is a small computer designed for automotive applications and connected to the ES910 via a CAN bus. It receives a GPS position that is generated by the simulation on the Xpack4 and passed trough the ES910. Using this

information the map software EB Assist ADTF by Electrobit, which runs on the car PC, provides map data for the ES910.

A similar HiL setup also for a trajectory optimization and vehicle control use case is described in [54, pp. 181-192].

The HiL environment enables safe, reproducible and automatically performed tests. It also helps to reduce expensive testing time in the real vehicle because most specifications can be verified in advance [187, pp. 163-168].

Despite the great benefits of the in-the-Loop methods, they cannot completely replace real test drives. At further development stages new test scenarios that cause problems more likely occur in reality than in the already well-known and simplified simulation environment. Furthermore, some features of driver assistance systems require a subjective assessment [187, pp. 163-168].

 Real test drives were conducted on the WR itself as well as on the highway and at roundabouts nearby. The test drives could focus on determining various calibration parameters such as acceleration look-up tables, weighting factors of the trajectory optimization and parameters of the controller that computes the torque demand. Parameter settings for three different modes that correspond to different driving styles were derived. In a final acceptance test about ten employees of the project partner who work in automotive research and engineering tested the ALC on self chosen routes. *Adopted from [79]*.

# **6.2 System architecture and design**

 The software components of the ALC are denoted modules. The ALC system consists of route data module, parameter adaption module, trajectory module and controller module. Figure 6.3 depicts the architecture of ALC. The following sections describe the component design, i.e. the function each module performs. *Adopted from [79]*.

Figure 6.3: System architecture of the automated longitudinal control. *Adopted from [79].*

### **6.2.1 Route data module**

 The input to the route data module comprises six vectors that contain the values and positions of legal speed limits, the road curvature and the slope for the road section ahead of the vehicle in driving direction. Similar to a runlength encoding, the vectors contain only information for positions at which the corresponding quantity changes significantly. The map data is provided via the CAN bus by the navigation system. *Adopted from [79]*. Since no destination is specified, the navigation system only knows the route up to the next junction or roundabout. If all roads connected to the junction have the same average amount of traffic, the electronic horizon ends at this junction. If one road has a higher traffic density, the navigation system assumes that the driver takes this road because it is the most probable path (MPP).

Following the MPP repeatedly leads onto the next larger road and finally onto a highway. Unless the driver follows an assumed MPP, there will be a short time period with no available map data. The navigation system then recalculates a new path whereby the length of the electronic horizon quickly increases.

Sometimes the driver likes to turn at a junction but the navigation system assumes that the junction will be crossed straight. In such cases the ALC does not reduce the vehicle velocity sufficiently and the driver needs to brake. Since the ALC neither uses information about right of way nor can detect traffic signs or traffic lights via a camera, the driver usually has to override the ALC at junctions and roundabouts.

The length of the electronic horizon ranges from several hundred meters in urban areas over roughly 1 km on country roads to several kilometers on highways.

The route data module is executed every 500 ms and mainly converts the map data to a meter discrete representation for up to 3 km ahead of the vehicle position and provides three vectors that describe the courses of road slope, road curvature and legal speed limit. However, the input vectors of the legal speed limit can be modified before the conversion by three mechanisms:

First, the driver can overwrite the current legal speed limit of the map data by adding a positive or negative offset using buttons next to the steering wheel. This adaption functionality proves beneficial in situations in which the legal speed limit in the map data does not match the actual legal speed limit, for example at temporary construction sites.

Second, the positions of upcoming speed limits can be changed. If the legal speed limit is increased, the ALC will not start to accelerate the vehicle before the new traffic sign is passed because the planned trajectory does not exceed the generated upper speed limit. When leaving a city, this behavior is usually undesired by the driver and can provoke overtaking maneuvers of following vehicles. Therefore two parameters were added. Both describe a time offset, one for the case of an increase of the legal speed limit and one for a decrease of the legal speed limit. In both cases the corresponding parameter is multiplied with the lower of the two speed limits to determine the spatial distance by which

the position of the higher speed limit is shifted in the map data. This allows to implement a more realistic driving behavior or adaption to driver preferences. For example, it can be achieved that the ALC already starts to accelerate when the driver sees the traffic sign that indicates a higher legal speed limit.

Third, an upper limit is enforced on the legal speed limit depending on the driving mode of the ALC selected by the driver. This is needed on highways because there is not always a legal speed limit.

The route data module was implemented by D. Dörr [22, pp. 23, 24]. The author of this work added the time offset parameters and logic for processing roundabout data.

### **6.2.2 Parameter adaption module**

 The parameter adaption module updates the ATFM and AEPM described in Subsection 4.7.1 and Subsection 4.7.2, respectively, every 50 ms using vehicle data from the CAN bus. During the ATFM update one KF iteration is performed and during the AEPM update the FB-KRLS takes into account a new measurement.

An update of the ATFM requires the traction force *F*trac, the vehicle velocity vVhcl and the sensor longitudinal acceleration *a*<sup>x</sup> and an update of the AEPM the electrical traction power *P*trac,elec, vVhcl and *a*x. While vVhcl and *a*<sup>x</sup> are measurement signals on the CAN bus, *F*trac needs to be approximately computed from the motor torque signals using (4.25). *P*trac,elec can be determined from the voltages and currents of the electric motors that are available on the CAN bus.

Before CAN signals are fed to an estimator or used to calculate the input quantity for an estimator, signal noise is removed using a polynomial function approximation method [145], also denoted polynomial Kalman smoother [144].

In two cases the models are not adapted: First, during a vehicle standstill because the missing excitation in the data can cause estimations to diverge from their true value. Second, if CAN signals indicate that there is significant hydraulic brake pressure because the models only consider braking via recuperation and are not valid when hydraulic brakes are active.

Output of the parameter adaption module are updated parameters of ATFM and AEPM. The parameter adaption module including ATFM is from F. Bleimund [22, pp. 24-29]. The author of this work added the AEPM. *Adopted from [79]*.

### **6.2.3 Trajectory module**

 The trajectory module performs the tasks that Chapter 5 described in detail. These tasks include generating an upper speed limit and planning a trajectory. Inputs are the map data from the route data module, parameters of the AEPM provided by the parameter adaption module, vehicle data and the selected driving mode. The selected driving mode determines which longitudinal acceleration look-up table is used to compute the speed limit from map data with desired acceleration. Furthermore, in driving mode "Normal" and driving mode "Sport" RBA performs the trajectory optimization whereas in driving mode "Range" NRBA optimizes the trajectory using the AEPM, whereby the absolute value of the electrical traction power is penalized strongly.

The trajectory module is called every 500 ms and then can either continue planning the current trajectory or start planning a new trajectory.

Continuing planning the current trajectory means that the module performs additional iterations of RBA or NRBA until it reaches the iteration limit for the current module call or the optimized trajectory reaches the end of the electronic horizon (c.f. Figure 5.3).

Planning of a new trajectory is only triggered if distance limit or time limit since the last beginning of a trajectory planning have been exceeded, if the driver has stopped overriding the ALC by pressing the braking pedal or accelerator pedal or if data of the curvature vector has changed more than a threshold since the last module call indicating that the driver has chosen a route that differs from the MPP. Additionally, the length of the electronic horizon must exceed a specified lower limit to enable planning of a new trajectory. *Adopted from [79]*.

In two cases, vTJY and *a*TJY of the new trajectory are projected onto the vehicle velocity vVhcl and vehicle longitudinal acceleration v˙Vhcl,x according to Subsection 5.5.1: First, if there is no valid previous trajectory, e.g. because map data was temporarily not available, and second, if the current vehicle velocity deviates more than a certain threshold from the planned velocity vTJY, e.g. because the vehicle is starting from a standstill or because of an intervention by the driver.

In all other cases vTJY(*t*1), *a*TJY(*t*1) and *j*TJY(*t*1) with *t*<sup>1</sup> = 0 of the new trajectory are projected onto vTJY(*t* ∗ ), *a*TJY(*t* ∗ ) and *j*TJY(*t* ∗ ) of the current trajectory at its last evaluation point *t* ∗ according to Subsection 5.5.2 in order to achieve a smooth connection between both trajectories at *t* ∗ .

The knot vector κ and estimated control point vector <sup>x</sup><sup>ˆ</sup> are the outputs of the trajectory module.

### **6.2.4 Controller module**

 Every 20 ms the controller modules computes the desired velocity vdes and desired acceleration *a*des and translates these quantities into a motor torque demand *T*des that causes the vehicle to track the velocity trajectory. In most cases vdes and *a*des equal the trajectory velocity vTJY and the trajectory acceleration *a*TJY, respectively. vTJY and *a*TJY are determined by evaluating the trajectory defined by the knot vector and estimated control point vector from the trajectory module at the current point in time measured starting from the last trajectory planning start. *Adopted from [79]*.

However, the controller also comprises three functionalities of the trajectory module:


Since the controller module can alter the trajectory, the trajectory module accepts a control point vector coming from the controller module as input and interprets it as the previous trajectory. If there is no valid trajectory available for evaluation, the controller module causes the desired vehicle acceleration to quickly diminish and keeps the velocity constant. If no map data is available for several seconds, the ALC system deactivates itself.

The in this subsection above stated functionality was added by the author of this work. However, the architecture and control loop including the pilot control were adopted from F. Bleimund [22, pp. 35, 36].

 Figure 6.4 depicts the controller module architecture and the control loop. vdes and *a*des are inputs to a pilot control and a model predictive control (MPC).

Figure 6.4: Architecture of controller module and control loop. The pilot control generates an openloop motor torque demand Tdes,PC based on desired velocity vdes, desired acceleration <sup>a</sup>des and road slope angle <sup>α</sup> using the adaptive traction force model (ATFM). The model predictive control (MPC) computes a closed-loop torque demand Tdes,MPC to minimize the remaining deviation of vehicle velocity vVhcl and vehicle longitudinal acceleration v˙Vhcl,x from vdes and ades. *Adopted from [79].*

The pilot control contains the ATFM and computes an open-loop torque demand *<sup>T</sup>*des,PC based on road slope angle <sup>α</sup>, <sup>v</sup>des and *<sup>a</sup>*des. Due to imperfect map data, sensor data and ATFM, the vehicle velocity vVhcl deviates from vdes and the vehicle longitudinal acceleration v˙Vhcl,x deviates from *a*des. The twodimensional MPC computes a closed-loop torque demand *T*des,MPC in order to minimize these deviations.

The torque demand of the controller module *T*des is the sum of the pilot control torque demand *T*des,PC and the MPC torque demand *T*des,MPC and reaches the motor ECU via the CAN bus. The motor ECU computes the distribution of the torque demand between the front and rear motor using the strategy described in Section 4.3 and controls these actuators accordingly. If the torque demands indicates a deceleration request, it can additionally be allocated to the brake ECU that controls the hydraulic brakes.

The control loop is closed through the vehicle where CAN messages transfer the feedback information. A comprehensive review of trajectory tracking methods is provided in [40].

#### **Pilot control**

The pilot control determines an open-loop torque demand *T*des,PC with

$$T\_{\rm des,PC} = \frac{F\_{\rm trac}}{i\_{\rm G}} \cdot \frac{r\_{\rm dyn,FA} + r\_{\rm dyn,RA}}{2} \tag{6.1}$$

so that the vehicle roughly tracks <sup>v</sup>des and *<sup>a</sup>*des. *<sup>i</sup>*<sup>G</sup> is the gear ratio, *<sup>r</sup>*dyn,FA the dynamic front wheel radius and *<sup>r</sup>*dyn,RA the dynamic rear wheel radius. The traction force *F*trac results from the ATFM in Subsection 4.7.1:

*<sup>F</sup>*trac <sup>=</sup> (1, *<sup>a</sup>*x, <sup>v</sup> 2 des) <sup>|</sup> {z } =:CVhcl ·xVhcl (6.2)

The parameter adaption module provides the adapted vehicle parameter vector xVhcl. The vehicle motion vector CVhcl depends on vdes 2 and *a*x, whereby

$$a\_{\mathbf{x}} = a\_{\text{des}} + \mathbf{g} \cdot \sin(\alpha). \tag{6.3}$$

*Adopted from [79]*. According to Subsection 4.2.1, the road slope angle α can be computed from the road slope γ, which can be derived from map data or the signal of a slope estimator that is available on the CAN bus.

The simple calculations in the pilot control are very transparent, the estimated vehicle parameters can easily be limited to a valid range and the other values are measurements. The pilot control contributes to the torque demand to a large extent and serves for overcoming the majority of the sum of driving resistances. Using the pilot control allows to restrict the output of the MPC that closes the control loop to low absolute values without risiking a vehicle standstill at a slope. Restricting the MPC output seems beneficial from safety considerations because the MPC performs operations that can be numerically problematic such as dividing by potentially small values. Additionally it allows to apply a simpler vehicle model in the MPC for less computational effort.

#### **Model predictive control**

Model predictive control (MPC) is a direct trajectory optimization method that approximates the constrained infinite dimensional OCP from (2.1) by a finite dimensional problem that can be solved efficiently in real-time. The approximation occurs by discretizing the problem and solving it for a finite time horizon *T*.

MPC uses a system model to predict the future system behavior. A possible linear time invariant system model without uncertainties reads

$$\begin{aligned} \mathbf{x}\_{p+1} &= \mathcal{R}\mathbf{x}\_p + \mathcal{B}\mathbf{u}\_p \\ \mathbf{y}\_p &= \mathbf{C}\mathbf{x}\_p \end{aligned} \tag{6.4}$$

with system state xp, control input up, system output y<sup>p</sup> and discrete time index *p*. In each time step *t*, MPC solves the following optimization problem for the horizon *<sup>t</sup>* <sup>=</sup> <sup>0</sup>, δ*t*, . . ., *<sup>T</sup>* with cycle time δ*t*:

$$\begin{aligned} \hat{\mathfrak{u}}\_{l} &= \operatorname\*{arg\,min}\_{\mathbf{u}\_{l}} \mathcal{J}(\mathbf{x}\_{l}, \mathbf{u}\_{l}) \\ \mathcal{J}(\mathbf{x}\_{l}, \mathbf{u}\_{l}) &= \sum\_{i=0}^{T-1} \left[ (\mathbf{y}\_{l} - \mathbf{C}\mathbf{x}\_{i|l})^{\top} \otimes (\mathbf{y}\_{l} - \mathbf{C}\mathbf{x}\_{i|l}) + \boldsymbol{\mu}\_{i|l}^{\top} \mathcal{R}\mathbf{u}\_{i|l}^{\top} \right] \\ \text{subject to } \mathcal{F}\mathbf{x}\_{l} + \mathcal{G}\mathbf{u}\_{l} &\leq 1 \end{aligned} \tag{6.5}$$

Herein the control input sequence u<sup>t</sup> = {u0|<sup>t</sup> , <sup>u</sup>1|<sup>t</sup> , . . ., <sup>u</sup>T−1|t} over the horizon is the optimization variable. xi|<sup>t</sup> and ui|<sup>t</sup> denote the predicted values of x and u, respectively, for time *t* + *i* based on the information available at *t*, whereby x0|<sup>t</sup> = x<sup>t</sup> is assumed. Present and future set points are specified via the system output y<sup>t</sup> = {y0|<sup>t</sup> , <sup>y</sup>1|<sup>t</sup> , . . ., <sup>y</sup>T−1|t}.

The cost function J includes symmetric error weighting matrices R and Q, which are assumed positive definite and positive semidefinite, respectively.

either forwarded to a low-level feedback steering/acceleration controller or the

56 Integrated Trajectory Optimization 1431

The receding horizon approach is the gist of model-predictive control (MPC, see, e.g., Rawlings 2000), which makes a numerical optimization practical for closedloop control. Therein, in each step tk, the OCP is solved on a finite horizon T, which calculates the optimal open-loop trajectories <sup>x</sup>ð Þ<sup>τ</sup> over <sup>τ</sup> ½ tk, tk <sup>þ</sup> <sup>T</sup> , see Fig. 14. Only the first part of the optimal control ū(τ) is implemented on Δt. Right in time the new solution is available of the OCP that has been shifted by Δt. In classical MPC, at each tk the current plant state is fed back as the new initial state of the OCP. Altogether, this leads to a closed control loop that anticipates future events, such as

This procedure is completely compatible with trajectory optimization for vehi-

fundamentally change the OCP from one optimization step to the other.

Closed-form solutions from the calculus of variations are thereby often used to speed up dynamic programming. This can be in the form of a heuristic for an informed search (see, e.g., Ziegler et al. 2008) or so-called analytical expansions (Dolgov et al. 2010), both of which, roughly speaking, approximate the remaining trajectory and therefore extend the computable optimization horizon, see Table 1.

optimal input trajectory is directly fed to the vehicle actuators.

input and state saturation, and takes control actions accordingly.

5 Receding Horizon Optimization

Fig. 14 Receding horizon optimization with optimization length T, cycle time Δt, current time step tk, predicted and actual trajectory <sup>x</sup>, <sup>x</sup>, predicted and actual system input <sup>ū</sup>, <sup>u</sup> Figure 6.5: Model predictive control optimization horizon with time horizon <sup>T</sup>, cycle time δt, current time step tp, planned and actual state xˆ, x, planned and actual control input uˆ , u [187, p. 1431, adapted].

The matrices F and G allow for linear constraints with respect to x and u, respectively. The constraints are formulated as inequalities, which apply elementwise. **1** is a matrix of ones. For increased robustness and in order to avoid an empty set of feasible solutions, constraint softening can be applied. Thereby hard constraints become penality terms in the cost function analogously to the approach stated in Section 5.5 [61].

Of the optimal control sequence uˆ<sup>t</sup> only the first element uˆ <sup>0</sup>|<sup>t</sup> is applied to the system. At the following time step the process is repeated with the additional information xt+<sup>1</sup> on the system state.

Figure 6.5 illustrates this MPC approach, that creates a feedback which partially compensates for model inaccuracies and leads to a closed-loop control that can take anticipatory control actions for future set points and events. The abilities of looking ahead and considering constraints are also distinguishing features of MPC compared to a PID controller [187, pp. 1431-1432], [95, pp. 2, 13-16].

An analysis of different controller designs is provided by [169] and different vehicle models for controller design are stated in [136, p. 12].

Vehicle related MPC applications in literature include ACC systems, e.g. [13, 61, 112, 154]. These use MPC as a high level controller that calculates a sequence of acceleration commands for the ACC equipped vehicle so that it keeps the required distance to a vehicle ahead, whereas a low level controller tracks the acceleration sequence using commands to power train and brakes. In the MPC the vehicle is represented with a first-order model because the low level controller cannot realize commands instantaneously due to the limited bandwidth of the vehicle.

In this work the MPC serves only as a low level controller and is additionally supported by the pilot control that considers the driving resistances. Furthermore it seems reasonable to assume that the research vehicle is capable of following power train and brake commands relatively quickly because of the high power to weight ratio and the missing delay from changing gears. For these reasons a vehicle model similar to the one in [116] is used. It is not a first-order model but a double integrator involving vehicle velocity vVhcl, vehicle longitudinal acceleration v˙Vhcl,x and vehicle longitudinal jerk v¨Vhcl,x. The forward difference approximations

$$\begin{split} \dot{\nu}\_{\text{Vhcl,x}}(t) &= \frac{\nu\_{\text{Vhcl}}(t + \delta t) - \nu\_{\text{Vhcl}}(t)}{\delta t} \\ \ddot{\nu}\_{\text{Vhcl,x}}(t) &= \frac{\dot{\nu}\_{\text{Vhcl,x}}(t + \delta t) - \dot{\nu}\_{\text{Vhcl,x}}(t)}{\delta t} \end{split} \tag{6.6}$$

give the time-discrete state-space representation of the vehicle model, in which v¨Vhcl,x is the control input [13, 116]:

$$
\begin{pmatrix} \mathsf{V} \mathsf{V} \mathsf{hcl}, t+1 \\\\ \dot{\mathsf{v}} \mathsf{V} \mathsf{hcl}, \mathsf{x}, t+1 \end{pmatrix} = \begin{pmatrix} 1 & \delta t \\ 0 & 1 \end{pmatrix} \cdot \begin{pmatrix} \mathsf{V} \mathsf{V} \mathsf{hcl}, t \\\\ \dot{\mathsf{v}} \mathsf{V} \mathsf{hcl}, \mathsf{x}, t \end{pmatrix} + \begin{pmatrix} 0 \\\\ 1 \end{pmatrix} \cdot \ddot{\mathsf{v}}\_{\mathsf{V} \mathsf{hcl}, \mathsf{x}, t} \tag{6.7}
$$

The system state comprises vehicle velocity and acceleration and should be equal to the set points for the desired velocity vdes and desired acceleration *a*des. Hence, the quantities in the state-space model of (6.4) read:

$$\begin{aligned} \mathcal{H} &= \begin{pmatrix} 1 & \delta t \\ 0 & 1 \end{pmatrix}, \quad \mathcal{B} = \begin{pmatrix} 0 \\ 1 \end{pmatrix}, \quad \mathcal{C} = \begin{pmatrix} 1 & 0 \\ 0 & 1 \end{pmatrix}, \\\ \mathbf{u}\_I &= \ddot{\mathbf{v}}\_{\text{Vhcl,x}}, \quad \mathbf{x}\_I = \begin{pmatrix} \mathbf{v}\_{\text{Vhcl}} \\ \dot{\mathbf{v}}\_{\text{Vhcl,x}} \end{pmatrix}, \quad \mathbf{y} = \begin{pmatrix} \mathbf{v}\_{\text{des}} \\ a\_{\text{des}} \end{pmatrix} \end{aligned} \tag{6.8}$$

Simplifying *F*trac in (6.1) to *F*trac = *m*vhcl · v˙Vhcl,x and calculating the temporal derivative shows that the control input u defined as the vehicle longitudinal jerk v¨Vhcl,x translates to changes of MPC torque demand, *T*des,MPC:

$$\frac{\text{d}T\_{\text{des,MPC}}}{\text{d}t} = \frac{m\_{\text{vhcl}} \cdot \vec{v}\_{\text{Vhcl,x}}}{i\_{\text{G}}} \cdot \frac{r\_{\text{dyn, FA}} + r\_{\text{dyn, RA}}}{2} \tag{6.9}$$

Discretization of the above equation using the backward difference approximation gives *T*des,MPC for the current time step:

$$T\_{\rm des,MPC,t} = T\_{\rm des,MPC,t-1} + \ddot{\imath}\_{\rm Vchcl,x,t} \cdot \frac{m\_{\rm vhcl} \cdot \delta t}{i\_{\rm G}} \cdot \frac{r\_{\rm dyn,FA} + r\_{\rm dyn,RA}}{2} \tag{6.10}$$

Thereby *<sup>T</sup>*des,MPC,<sup>0</sup> <sup>=</sup> <sup>0</sup> and <sup>δ</sup>*<sup>t</sup>* <sup>=</sup> <sup>0</sup>.2 s are used. The change of *<sup>T</sup>*des,MPC in each δ*<sup>t</sup>* is limited to [∆*T*min, <sup>∆</sup>*T*max] <sup>=</sup> [−50 Nm, <sup>+</sup>50 Nm] with following matrices in (6.5):

$$\mathcal{F} = \begin{pmatrix} 0 & 0\\ 0 & 0 \end{pmatrix}, \mathcal{G} = \begin{pmatrix} -\frac{m\_{\text{vla}1} \cdot \delta t \cdot (r\_{\text{dyn,FA}} + r\_{\text{dyn,RA}})}{2 \cdot i\_{\text{G}} \cdot \Delta T\_{\text{min}}}\\ \frac{m\_{\text{vla}1} \cdot \delta t \cdot (r\_{\text{dyn,FA}} + r\_{\text{dyn,RA}})}{2 \cdot i\_{\text{G}} \cdot \Delta T\_{\text{max}}} \end{pmatrix} \tag{6.11}$$

Furthermore, the R describing the cost of the jerk and Q describing the cost of deviating from the trajectory velocity and the trajectory acceleration on its main diagonal elements are set to

$$\mathcal{R} = 10^{-4}, \quad \mathcal{Q} = \begin{pmatrix} 20 & 1 \end{pmatrix} \cdot I. \tag{6.12}$$

167

The horizon comprises the current time step *t* and nine future time steps, which results in 1.8 s look ahead of the MPC. With a previously in *e-generation* applied PID controller uncomfortable oscillations between acceleration and deceleration occured on test drives while driving downhill with roughly constant velocity. After the PID controller had been replaced with the MPC, these oscillations were not noticed anymore.

However, the computational effort of MPC is usually much larger than that of the PID controller and depends on the system model and the prediction horizon [147]. Problem (6.5) has a convex quadratic objective function and linear constraints, which allows using various QP solvers based on active set methods or interior point methods for performing the online MPC optimization.

This work uses the general purpose QP solver provided by [94] with minor modifications that consist of rewriting instructions that are incompatible with compilation for the target hardware and of removing functionality and dependencies that are not needed to solve the above stated problem. Modifications of the solver, first implementations and tests of promising state-space models within a provided simplified control loop were done by C. Lee [98] during his time as student assistant that started after completion of his Master's thesis. Parameter tuning and closed-loop testing within the ALC in simulated and real test drives were done by the author of this work.

The disadvantage of general purpose QP solvers is that they do not exploit the special MPC problem structure and therefore using them might prevent the application of MPC to problems with a high sample rate, high-dimensional model, or long time horizon *T*. For example, the computational effort of both active set and interior point methods increases roughly cubically with *T*, whereas the effort of algorithms that exploit sparse matrices increases only linearly [95, p. 42], [147]. A survey of technologies for linear and nonlinear MPC is given by [137] and [171] provides a comparison between PID controller, linear MPC and nonlinear MPC.

# **6.3 Energy-saving potential and effects of parameters**

 This section investigates the energy-saving potential using the automated longitudinal control (ALC) with different parameter settings compared to drives with manual longitudinal control (MLC) on the Weissach route (WR) defined in Section 4.6 in simulations.

## **Automated longitudinal control setup and data generation**

The varied parameters are the temporal distance of neighboring knots <sup>∆</sup>*t*κ , the number of spline intervals *I*, the temporal safety margin to upper speed limit ∆*t*Lim,TJY and the weight of power error square *R* −1 P .

Simulations of the ALC are conducted for all possible parameter combinations that result from <sup>∆</sup>*t*κ <sup>=</sup> {1.5 s, 3 s}, *<sup>I</sup>* <sup>=</sup> {1, <sup>3</sup>}, <sup>∆</sup>*t*Lim,TJY <sup>=</sup> {1 s, 2 s} and *<sup>R</sup>* −1 P = { 1 <sup>10000</sup>, 1 <sup>5000</sup>, 1 <sup>1000</sup>, 1 <sup>500</sup>, 1 <sup>100</sup>, 1 <sup>50</sup> }. *Adopted from [79]*.

During a simulation of the ALC on the WR, the position *s* on the WR is calculated by integrating the vehicle velocity over time. With *s* the corresponding road curvature and road grade are determined from the map data of the WR.

Section 5.4 applies the open-loop reference model from Section 4.5 in a backward simulation in order to calculate the energy consumption that results when the vehicle tracks a planned velocity trajectory perfectly. The current section considers the whole ALC including its controller module and calculates the energy consumption that results from the actually realized velocity trajectory using the closed-loop reference model from Section 4.5 in a forward simulation.

 In order to compare different ALC settings, the required trip time *t*Trip for completing the WR is converted into the average velocity by dividing *t*Trip

Figure 6.6: Average energy consumption vs. average velocity on the Weissach route with automated longitudinal control (ALC) under different parameter settings in comparison to adapted and resimulated real drive with manual longitudinal control (MLC). *Adopted from [79].*

by the length of the WR and the energy consumption is scaled to the energy consumption per 100 km. This approach allows to summarize each simulation in a single data point in Figure 6.6, which depicts energy consumption versus average velocity. The lines are approximations of data points differing only in *R* −1 P by quadratic polynomials. *Adopted from [79]*.

For some parameter combinations the values of *R* −1 P are high or low enough to make the MPF instable. In such cases the resulting energy consumption is very high while the average velocity is either very low or very high. Such data points are excluded from Figure 6.6. Reduced convergence radii are noticed mainly for the combinations that include <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 2 s and <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s. <sup>∆</sup>*t*Lim,TJY influences the data provided to the MPF and for nonlinear filters the covariance also depends on the data. If <sup>∆</sup>*t*κ is reduced while <sup>∆</sup>*t*It is kept constant, the covariance matrix entries tend to remain larger. This suggests that for these settings, the MPF parameter Q L , Q N need to be reduced.

### **Manual longitudinal control setup and data generation**

 For comparison with a drive with MLC, a real test drive on the WR with MLC was performed and CAN data was recorded. From the recorded data, only the vehicle velocity vVhcl with a sample rate of 100 Hz was further used. By integrating vVhcl over time, the corresponding position on the WR for each data point is computed. Time information is then discarded because the following modifications of vVhcl do not depend on time:

First vVhcl is multiplied with a factor around one in order to achieve MLC drives with various average velocities in the same range as the average velocities of the ALC simulations.

During the real test drive the driver had to stop the vehicle at junctions in order to check for other traffic participants with right of way. When the vehicle has to come to a standstill and drive off again this increases both the energy consumption and time required for completing the WR.

The simulation environment neglects right of way and other traffic participants do not occur. Therefore the vehicle passes junctions with a low speed without coming to a standstill. In order to not disadvantage MLC drives in the comparison, position dependent lower limits on vVhcl are applied to MLC drives. These lower limits coincide with the velocities the ALC chooses at the corresponding junction. Varying ALC parameter settings influences the velocity with ALC at the occuring junctions only slightly.

During the real test drive there was few traffic and only once during a short period the driver was restricted in the choice of vehicle velocity because of a vehicle ahead. At the corresponding position the recorded vVhcl data was adapted to a typical ALC behavior.

The ALC computes an upper speed limit for the route ahead, which among others depends on the specified maximum absolute value of lateral acceleration v˙Vhcl,y,max. The originally recorded vVhcl results in a lateral acceleration larger than the maximum absolute value of lateral acceleration for the ALC. Increasing v˙Vhcl,y,max leads to higher average velocity and less energy consumption. In order to compare ALC and real drive under same conditions, an upper limit is enforced on the vVhcl of the MLC drives. The upper limit is the speed limit from map data with desired acceleration.

After multiplying the recorded vVhcl with a factor around one and imposing the various limits the corresponding time *t* is re-calculated using *t* = s vVhcl and the MLC energy consumption is determined with the open-loop reference model. Figure 6.6 depicts the resulting data points and quadratic approximation for the MLC in black and by a solid line.

The quadratic approximation line serves as a benchmark. The ALC data points should be below this line, meaning that the ALC achieves the same average velocity with less energy consumption or a higher average velocity for the same energy consumption.

Initial evaluations of the energy consumption of MLC and ALC in a non-final setup were performed by A. Thorgeirsson [173] as a student assistant.

## **Parameter effects**

#### **Temporal distance of neighboring knots**

Approximations for data points that differ only in the temporal distance of neighboring knots <sup>∆</sup>*t*κ and weight of power error square *<sup>R</sup>* −1 P indicate higher average velocities and mostly also higher energy consumptions for <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s than for <sup>∆</sup>*t*κ <sup>=</sup> 3 s. The reason for this is that trajectories with larger <sup>∆</sup>*t*κ cannot follow the upper speed limit as closely because their degrees of freedom are temporally less dense. Hence, the vehicle is generally slower and omits some inefficient velocity peaks. *Adopted from [79]*.

#### **Number of spline intervals**

When the number of spline intervals *I* is increased, the MPF can adjust the trajectory in more previous trajectory intervals. When *R* −1 P is chosen high for strong penalization of the electrical traction power, a large *I* should be beneficial because the MPF can for instance retrospectively reduce the acceleration if the road grade suddenly increases and maintaining the acceleration would lead to large power demand and therefore large power loss.

However, similar to Chapter 3 and Chapter 5 the simulation results do not support this statement. All approximations for <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s and *<sup>I</sup>* <sup>=</sup> <sup>1</sup> are below those for <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s and *<sup>I</sup>* <sup>=</sup> <sup>3</sup>. For <sup>∆</sup>*t*κ <sup>=</sup> 3 s the ranges of the average velocities of the approximations for *I* = 1 and *I* = 3 do not always overlap but they can still be compared using their positions with respect to the MLC approximation. Using this criterion the approximations for *I* = 1 are always better.

The reason for the worse results with larger *I* can be explained by the fact that the PF in the MPF uses a set of particles, each of which is a possible trajectory control point vector. The number of particles equals 100 for all simulations. For larger *I*, each particle comprises more dimensions and therefore each particle dimension is sampled less densely.

In order to keep the sampling density in each dimension constant, the number of particles needs to be increased exponentially with *I*. Since the computational effort increases linearly with the number of particles, this quickly becomes infeasible especially with computation time restrictions. From these considerations and the simulation results, it seems beneficial to only use *I* = 1 in combination with MPF and power penalization. Section 5.3 investigated the effect of increasing *I* in combination with the KF.

Furthermore, with *I* = 3 there is less potential for an effect from varying weight *R* −1 P . Moreover, the approximation for <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 2 s, <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s and *<sup>I</sup>* <sup>=</sup> <sup>1</sup> comprises a larger range of average velocities and energy consumptions than the corresponding approximations for <sup>∆</sup>*t*κ <sup>=</sup> 3 s.

#### **Temporal safety margin to upper speed limit**

When the temporal safety margin to upper speed limit ∆*t*Lim,TJY is decreased, the trajectory follows the speed limit from map data with desired acceleration vLim,Map,v˙ more closely which increases the average verlocity. Furthermore, the trajectory can follow short lasting inefficient velocity peaks which increases energy consumption. Both these effects can be observed for the setting <sup>∆</sup>*t*κ <sup>=</sup> 3 s and *I* = 3, which achieve comparatively low average velocities. For the other settings, however, the average velocity remains almost unchanged but the energy consumption decreases. Lowering ∆*t*Lim,TJY also increases the trajectory velocity at local minima of vLim,Map,v˙ and can cause the trajectory to exceed vLim,Map,v˙ at its local minima. Such a behavior decreases energy consumption and increases average velocity at the same time at the expense of driving safety. The increase in average velocity is not observed though.

vLim,Map,v˙ is an upper speed limit calculated using parameters that are determined in real test drives. Slightly exceeding vLim,Map,v˙ is in most cases not noticed but should be avoided because the extent to which the trajectory exceeds vLim,Map,v˙ is not clearly defined. Figures in Chapter 5 demonstrate that trajectories with ∆*t*Lim,TJY = 1 s stay below vLim,Map,v˙ . Figure 5.4 indicates that even with the combination <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s, *<sup>I</sup>* <sup>=</sup> <sup>5</sup> and <sup>∆</sup>*t*κ <sup>=</sup> 5 s the trajectory remains below vLim,Map,v˙ .

For a more complete assessment of the extent and relative frequency of vLim,Map,v˙ exceedances, Figure 6.7 shows histograms of the relative difference between the actual vehicle velocity vVhcl and vLim,Map,v˙ for the ∆*t*Lim,TJY = 1 s and <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 2 s with the other parameters <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s, *<sup>I</sup>* <sup>=</sup> <sup>1</sup> and *<sup>R</sup>* −1 P = 1 <sup>5000</sup> . The histograms reveal that there are exceedances which was to be expected because of the unconstrained trajectory optimization problem formulation. The majority of these remain below 2 %, which also includes the controller inaccuracies. Higher exceedances can be observed but their relative frequencies are too low to lead to a safety critical situation during the roughly 1400 s long drive. It also needs to be mentioned that map data itself is not perfect and can be deprecated or be unavailable, especially during the first seconds after the

Figure 6.7: Histograms of the relative frequencies of relative differences between vehicle velocity vVhcl and speed limit from map data with desired acceleration vLim,Map,v˙ for different temporal safety margin to upper speed limit ∆tLim,TJY with R −1 P = 1 <sup>5000</sup> , <sup>∆</sup>t<sup>κ</sup> <sup>=</sup> <sup>1</sup>.5 s and I = 1.

Figure 6.8: Histograms of the relative frequencies of relative differences between vehicle velocity vVhcl and speed limit from map data with desired acceleration vLim,Map,v˙ for different weight of power error square R −1 P with <sup>∆</sup>tLim,TJY <sup>=</sup> 1 s, <sup>∆</sup>t<sup>κ</sup> <sup>=</sup> <sup>1</sup>.5 s and <sup>I</sup> <sup>=</sup> <sup>1</sup>.

driver has taken a different route than expected by the system in real test drives, which poses a higher risk in practice. Nevertheless, a slight shift to higher velocities is noticeable when ∆*t*Lim,TJY is reduced from 2 s to 1 s, which leads to a reduction in the energy consumption if the exceedance occurs in curves.

Figure 6.8 shows histograms of the relative exceedance for two values of the weight of power error square *R* −1 P , whereby <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s, <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s and *I* = 1. The results indicate that stronger penalization of the electrical traction power mainly leads to an in general slower drive and is unlikely to cause an exceedance of vLim,Map,v˙ in curves for the chosen settings.

#### **Saving potential and assessment**

 For the parameter combination <sup>∆</sup>*t*Lim,TJY <sup>=</sup> 1 s, <sup>∆</sup>*t*κ <sup>=</sup> <sup>1</sup>.5 s and *<sup>I</sup>* <sup>=</sup> <sup>1</sup>, the ALC achieves on average the largest absolute energy savings with respect to MLC. With additionally *R* −1 P = 1 <sup>1000</sup> , the ALC requires <sup>3</sup>.4 % less energy than the MLC at the same average velocity of <sup>59</sup>.0 km/h, and for the same energy of <sup>16</sup>.9 kWh/100km the ALC achieves a <sup>2</sup>.6 % higher average velocity. Increasing *R* −1 P from <sup>1</sup> <sup>1000</sup> to <sup>1</sup> <sup>50</sup> , reduces the energy consumption by <sup>2</sup>.9 % while the average velocity decreases by <sup>3</sup>.1 %.

Table 2.2 on page 11 supports assessing the time and energy savings by summarizing the results for different types of vehicles, roads and ALCs. The results in the first three lines were obtained for the WR, however for a conventional vehicle. The saving potential of a BEV is much lower than for a conventional or hybrid vehicle because of three reasons:


inefficient energy conversion process usually also takes place for the operation of a BEV but it occurs earlier during the electricity production from non-renewable energy sources and is therefore less frequently considered. Since the mix of available energy sources varies with the region so does the efficiency of fuel and electricity generation. If these processes are efficient, the total efficiency also referred to as *well to wheel efficiency* can be twice as high for a BEV than for a conventional vehicle. The gear box in a BEV is also more efficient since it usually only has a fixed gear ratio and therefore much less moving parts [10].

• *Less degrees of freedom for optimization of driving stategy*: Since most BEVs have a fixed gear ratio the only degree of freedom is the choice of torque, which is however strongly coupled to the trip time and constrained quantities such as velocity and acceleration. Opposed to that, the power train of a conventional vehicle offers selected gear and clutch state as additional degrees of freedom for optimization. In a HEV even the way internal combustion engine, electric motor and battery work together can be chosen. For that reason the latter vehicle types benefit strongly from a look-ahead driving strategy.

Due to these aspects it is no surprise that the largest time saving of 21.13 % at the same energy consumption is stated for a HEV and the largest energy saving of 23.8 %, that however is accompanied by a 1.5 % longer trip time, is reported for a conventional vehicle. These large savings were achieved in investigations including country road and urban sections though.

For a BEV the results of Table 2.2 state average energy savings of 1.21 % on a 1400 km long highway section and of 5.6 % on an only 400 m artificial road profile which comes closest to an urban section due to its velocity of roughly 30 km/<sup>h</sup> and maximum absolute slopes of 10 %. Compared to these results, the savings achieved by the ALC of this work seem reasonable. *Adopted from [79]*.

# **6.4 Technical contribution**

On the level of driver assistance systems, the contribution of this work is the development of an energy-efficient automated longitudinal control for a BEV and its parametrization and test in simulations and real test drives. The developed ALC system shares some similiarities with the solution presented in [114, 139, 183]. All systems use predictive route data and measurements of a radar sensor. Each system performs an ACC functionality that is extended by the use of map data. This enables the system to choose a suitable velocity depending on legal speed limits and road geometry.

The systems can provide the most benefit on winding country roads, on which the standard ACC requires frequent manual adaption of the desired velocity. The systems can also be used on highways and within cities. However, on highways there is a comfort advantage over ACC only when the legal speed limit changes and within cities the driver frequently has to interven because the systems do no consider right of way or traffic signs. Furthermore, other traffic participants are only detected by a radar sensor.

Power train models enable the systems to determine a more energy efficient driving strategy compared to manual driving.

The differences and unique features of the system described in this work refer to the target vehicle and especially the approach for planning the course of velocity. The two target vehicles in [139] have an internal combustion engine and the two target vehicles in [183] a hybrid power train. Therefore rather complex planning approaches that consider gear selection are applied to derive an energy-efficient course of velocity. In contrast a BEV like the target vehicle for this work usually has a 1-speed gear box, i.e. a constant gear ratio. The proposed ALC includes a novel trajectory planning method that takes advantage of the simple BEV power train.

Compared to the ALC that was developed for the same research vehicle during the preceding project, the ALC in this work uses a trajectory with respect to time. Especially at low speeds the time-based approach enables a much more dynamic vehicle behavior, which is beneficial for driver acceptance. Furthermore, the ALC of this work considers the required electrical power explicitly and allows to optimize the trajectory not only with respect to travel time and comfort but also with respect to driving efficiency.

In simulative investigations of the energy-saving potential for a BEV the ALC requires up to <sup>3</sup>.4 % less energy than the MLC for the same average velocity and achieves a <sup>2</sup>.6 % higher average velocity for the same energy consumption on the reference route.

In a final acceptance test about ten employees of the project partners who all work in automotive research and engineering tested the ALC on self chosen routes. The ALC yielded throughout good comments, which especially addressed the comfortable and smooth driving style.

The developed system can be used on its own or in combination with other assistance systems such as an automated lateral control. *Adopted from [79]*.

# **7 Conclusions**

# **7.1 Summary**

This work describes novel methods for the general problem of approximating an unbounded number of data points using a B-spline function in the linear and nonlinear weighted least squares (WLS) sense. The developed methods are based on iterative algorithms for state estimation and their computational effort increases linearly with the number of data points. The methods can adjust the bounded definition range of a B-spline function during run-time if this is required to approximate the latest data point.

The approximation problem is reformulated as a trajectory optimization problem such that the approximation methods compute a velocity trajectory with respect to time using data points created from map data. The developed trajectory optimization methods fall into the category of direct methods and its effort increases linearly with the temporal length of the planned trajectory. The combination with an adaptive model that describes the power train properties of the battery electric vehicle (BEV), allows to plan velocity trajectories whose resulting energy consumption varies depending on the chosen relative weighting of different target criteria.

The trajectory optimization is extented to an assistance system for automated longitudinal control (ALC) that is tested in simulation as well as in real test drives. In simulations on a reference route the developed ALC is compared to a recorded and re-simulated real drive with manual longitudinal control (MLC) and can achieve better ratios between average velocity and energy consumption than MLC.

# **7.2 Contribution**

The contribution of this work is threefold and comprises approximation algorithms, a trajectory planning method and a system for energy-efficient ALC of a BEV.

Iterative algorithms are proposed for the approximation of an unbounded set of data as it occurs in online applications. The first algorithm denoted recursive B-spline approximation (RBA) and prepublished in [80] solves a linear WLS approximation problem iteratively using a Kalman filter (KF). The second algorithm is termed nonlinear recursive B-spline approximation (NRBA) and prepublished in [78]. NRBA is generalization of RBA for nonlinear weighted least squares (NWLS) problems and uses a marginalized particle filter (MPF). In the MPF a particle filter (PF) deals with the nonlinear subproblem, whereas a linear KF solves the linear subproblem optimally.

RBA and NRBA include a novel shift operation which allows to shift the bounded definition range of a B-spline function during run-time such that it is always possible to take into account the latest data point. With previous recursive algorithms the approximation interval is fixed during run-time and if data outside this interval occurs, it cannot be considered. Moreover, the shift operation allows to decrease the size of vectors in the filters in order to reduce computational effort in both offline and online applications. In offline applications all data points are available at once, so their number is bounded and batch processing in one step is possible. In contrast, in online applications, additional data points are oberved in each time step, therefore a previously calculated solution must be updated with each new observation.

The exponential growth of computational effort with increasing time horizon limits the application of direct trajectory optimization approaches to short time horizons. This work presents a direct trajectory optimization method whose computational effort only grows linearly with the number of function parameters or the time horizon.

This substantial saving in computational effort is achieved by taking adantage of the comparatively simple power train of a BEV with fixed gear ratio and by formulating constraints coming from the vehicle dynamics or the environment mainly as soft constraints. The resulting nonlinear optimization problem is formulated as an NWLS problem and solved using NRBA. The trajectory can be optimized with respect to travel time, comfort and energy consumption. Without consideration of energy consumption the nonlinear optimization problem can be approximated by a quadratic optimization problem that is solved with less computational effort using RBA. *Adopted from [79]*.

The trajectory is defined by a B-spline function that describes the desired vehicle velocity with respect to time. The temporal length of the trajectory increases with the iterations and the number of function parameters does not need to be bounded. If the temporal length of the trajectory must be increased, function parameters can be added during the optimization. After each iteration an intermediate result is available. The optimization can be paused and continued in accordance with computation time constraints.

 Based on the novel trajectory planning approach a system for energy-efficient automated longitudinal control of a BEV is developed and tested in simulation and real test drives. The system can provide the most benefit on winding country roads, but can also be used on highways and within cities.

In simulations on a chosen reference route the ALC needs up to <sup>3</sup>.4 % less energy than the MLC for the same average velocity and achieves a <sup>2</sup>.6 % higher average velocity for the same energy consumption as with MLC.

For additional subjective impressions, employees in automotive research and engineering tested the ALC on other, individually chosen routes. The throughout good reviews especially mentioned the comfortable and smooth driving style of the ALC. *Adopted from [79]*.

## **7.3 Suggestions for further research**

The approximation algorithms RBA and NRBA have only been investigated for B-spline functions. According to Section 2.3, they can be extended to multidimensional control points for approximation of parametric curves and surfaces. Furthermore, RBA and NRBA can be transferred to other spline representations which offer local control and arise from a linear combination of basis functions and control points.

 Additionally, the modular concept of these algorithms allows to replace the included filters. The KF used for the linear approximation is known to be optimal, but for the PF and the MPF used for the nonlinear approximation various improvements are available in literature. For example, [203] proposes a MPF that determines the particles of the PF using particle swarm optimization and achieves better results at lower effort compared to both a standard MPF and PF. *Adopted from [78]*. Even more promising for the nonlinear approximation seems using a kernel adaptive filter as it does not suffer from the curse of dimensionality and requires only solving a convex problem, which is very beneficial for real-time applications (c.f. Section 2.4 and Subsection 4.7.2).

Regarding trajectory planning the computation of the temporal safety margin to upper speed limit can be enhanced such that the trajectory follows the upper speed limit closer at high speeds while still remaining below local minima of the upper speed limit.

A more comprehensive improvement consists of an adaptive selection of the values of temporal distance of neighboring knots, the temporal safety margin to upper speed limit and the temporal distance of neighboring data points so that the trajectory degrees of freedom per trajectory time unit are not constant but depend on the driving situation. For example, on country roads and highways with simple shape of the upper speed limit, temporal distance of neighboring knots and temporal distance of neighboring data points can be increased. Then, for the same computational effort, longer, more farsighted trajectories can be planned because the tractory degrees of freedom are temporally less dense. The resulting larger time horizon, that can be optimized simultaneously by the proposed local trajectory optimization approach, facilitates achieving higher reductions of energy consumption. In contrast, in situations with complicated upper speed limit or at low speeds, the values can be reduced to enable a more dynamic trajectory. Analogously, the relative weighting of the trajectory optimization can be designed adaptive.

Moreover, additional information can be used for calculating the upper speed limit. This can be either data from vehicle-based sensors such as cameras for detecting traffic signs and other traffic participants like pedestrians or it can be messages of other vehicles or the infrastructure. In particular the driving style should be adapted according to the present weather and friction coefficient between tire and road [19].

The mentioned improvements of the trajectory planning directly translate to improvements of the ALC system which can also be extended by other systems such as a lateral control system.

Alternatively, the trajectory optimization approach can be combined with Dynamic Programming (DP), as [187, pp. 1430-1431] suggests and Table 2.5 illustrates, in order to solve more complex problems, for which the presented filter-based approach on its own seems not suitable.

In such a case, DP can still plan in the spatial domain, which can be even more coarsely discretized for less computational effort because a rough long-term trajectory is sufficient. From this long-term trajectory the filter-based trajectory optimization approach creates a smooth trajectory in time domain with the benefits mentioned in Section 5.2 that can represent the vehicle velocity starting from zero so that the case of driving off can also be covered by the trajectory optimization.

# **List of Figures**




# **List of Tables**


# **List of Algorithms**


# **References**


and policymakers". In: *Transportation Research Part D: Transport and Environment* 53 (2017), pp. 234–248. DOI: 10.1016/j.trd.2017.04.020.


# **Publications**

## **Peer-reviewed journal papers**


### **Source code**

[75] J. Jauch. *ALC Matlab files*. 2019. DOI: 10.5281/zenodo.3334806. URL: http://github.com/JensJauch/ALC/releases.


## **Technical reports**

[41] D. Dörr, B. Fath, J. Jauch, and M. Frey. *Verbundprojekt: "Gesamtfahrzeugintegration innovativer Konzepte für ein effizientes und performantes E-Fahrzeug (e-volution)", Teil vorhaben KIT: "Assistenzsysteme für effiziente und performante Elektrofahrzeuge" : Schlussbericht*. Tech. rep. Karlsruher Institut für Technologie (KIT), 2017.

# **Supervised student theses**


### **Karlsruher Schriftenreihe Fahrzeugsystemtechnik FAST Institut für Fahrzeugsystemtechnik (ISSN 1869-6058)**

Eine vollständige Übersicht der Bände finden Sie im Verlagsshop



Die Bände sind unter www.ksp.kit.edu als PDF frei verfügbar oder als Druckausgabe bestellbar.


Die Bände sind unter www.ksp.kit.edu als PDF frei verfügbar oder als Druckausgabe bestellbar.


**Band 112** Jens Jauch T**rajectory optimization based on recursive B-spline approximation for automated longitudinal control of a battery electric vehicle** ISBN 978-3-7315-1332-2

Die Bände sind unter www.ksp.kit.edu als PDF frei verfügbar oder als Druckausgabe bestellbar.

### Karlsruher Schriftenreihe Fahrzeugsystemtechnik

This work describes a method for approximating an unbounded number of data points in the linear and nonlinear weighted least squares sense using a B-spline function. The method can shift the bounded B-spline function definition range during run-time and its computational effort increases linearly with the data.

The approximation method is applied for planning velocity trajectories with respect to time using map data. The effort of the resulting direct trajectory optimization method grows linearly with the unbounded trajectory length. The combination with a power train model of a battery electric vehicle allows to optimize velocity trajectories with respect to travel time, comfort and energy consumption.

The trajectory optimization method is extended to a driver assistance system for automated vehicle longitudinal control that is tested in simulated and real drives. In simulated drives the system achieves a higher energy efficiency and time efficiency than a human driver.

ISSN 1869-6058 ISBN 978-3-7315-1332-2